diff --git a/CMakeLists.txt b/CMakeLists.txt index f058f3ac56b4..4d0d21389236 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1325,7 +1325,7 @@ if(WITH_BULLET AND WITH_SYSTEM_BULLET) set(WITH_BULLET OFF) endif() else() - set(BULLET_INCLUDE_DIRS "${CMAKE_SOURCE_DIR}/extern/bullet2/src") + set(BULLET_INCLUDE_DIRS "${CMAKE_SOURCE_DIR}/extern/bullet/src") # set(BULLET_LIBRARIES "") endif() diff --git a/doc/python_api/rst/bge_types/bge.types.KX_CharacterWrapper.rst b/doc/python_api/rst/bge_types/bge.types.KX_CharacterWrapper.rst index 721ce73d36e5..f4b12ed75b3a 100644 --- a/doc/python_api/rst/bge_types/bge.types.KX_CharacterWrapper.rst +++ b/doc/python_api/rst/bge_types/bge.types.KX_CharacterWrapper.rst @@ -17,7 +17,7 @@ base class --- :class:`EXP_PyObjectPlus` The gravity value used for the character. - :type: float + :type: :class:`mathutils.Vector` .. attribute:: fallSpeed diff --git a/extern/CMakeLists.txt b/extern/CMakeLists.txt index 2e8589ffd176..cc463cb2337a 100644 --- a/extern/CMakeLists.txt +++ b/extern/CMakeLists.txt @@ -42,7 +42,7 @@ add_subdirectory(wcwidth) if(WITH_BULLET) if(NOT WITH_SYSTEM_BULLET) - add_subdirectory(bullet2) + add_subdirectory(bullet) endif() endif() diff --git a/extern/bullet/AUTHORS.txt b/extern/bullet/AUTHORS.txt new file mode 100644 index 000000000000..1bff63207795 --- /dev/null +++ b/extern/bullet/AUTHORS.txt @@ -0,0 +1,45 @@ +Bullet Physics is created by Erwin Coumans with contributions from the following authors / copyright holders: + +AMD +Apple +Yunfei Bai +Steve Baker +Gino van den Bergen +Jeff Bingham +Nicola Candussi +Erin Catto +Lawrence Chai +Erwin Coumans +Disney Animation +Benjamin Ellenberger +Christer Ericson +Google +Dirk Gregorius +Marcus Hennix +Jasmine Hsu +MBSim Development Team +Takahiro Harada +Simon Hobbs +John Hsu +Ole Kniemeyer +Jay Lee +Francisco Leon +lunkhound +Vsevolod Klementjev +Phil Knight +John McCutchan +Steven Peters +Roman Ponomarev +Nathanael Presson +Gabor PUHR +Arthur Shek +Russel Smith +Sony +Jakub Stephien +Marten Svanfeldt +Jie Tan +Pierre Terdiman +Steven Thompson +Tamas Umenhoffer + +If your name is missing, please send an email to erwin.coumans@gmail.com or file an issue at http://github.com/bulletphysics/bullet3 diff --git a/extern/bullet2/CMakeLists.txt b/extern/bullet/CMakeLists.txt similarity index 62% rename from extern/bullet2/CMakeLists.txt rename to extern/bullet/CMakeLists.txt index 949a8b01bd2a..4f4ff503b4bb 100644 --- a/extern/bullet2/CMakeLists.txt +++ b/extern/bullet/CMakeLists.txt @@ -29,389 +29,622 @@ set(INC ) set(INC_SYS - + ${TBB_INCLUDE_DIR} ) set(SRC - src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp + src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp + src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp + src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp + src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp + src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp + src/BulletDynamics/Featherstone/btMultiBody.cpp + src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp + src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp + src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp + src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp + src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp + src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp + src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp + src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp + src/BulletDynamics/Dynamics/btRigidBody.cpp + src/BulletDynamics/Character/btKinematicCharacterController.cpp + src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp + src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp + src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp + src/BulletDynamics/Vehicle/btWheelInfo.cpp + src/BulletDynamics/Vehicle/btRaycastVehicle.cpp + src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp + src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp + src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp + src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp + src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp + src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp + src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp + src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp + src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp + src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp + src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp + src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp + src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp + src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp + src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp + src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp + src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp + src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp + src/BulletInverseDynamics/MultiBodyTree.cpp + src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp + src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp + src/BulletInverseDynamics/IDMath.cpp + src/BulletSoftBody/btSoftBodyHelpers.cpp + src/BulletSoftBody/btSoftBody.cpp + src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp + src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp + src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp + src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp + src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp + src/BulletSoftBody/btSoftRigidDynamicsWorldMt.cpp + src/BulletSoftBody/btDefaultSoftBodySolver.cpp + src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp + src/Bullet3Geometry/b3GeometryUtil.cpp + src/Bullet3Geometry/b3ConvexHullComputer.cpp + src/clew/clew.c + src/Bullet3Common/b3Vector3.cpp + src/Bullet3Common/b3Logging.cpp + src/Bullet3Common/b3AlignedAllocator.cpp + src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp + src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp + src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp + src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp + src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp + src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp + src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp + src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.cpp + src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.cpp + src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.cpp + src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp + src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.cpp + src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp + src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.cpp + src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp + src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.cpp + src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp + src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp + src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.cpp + src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp + src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.cpp + src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp + src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.cpp + src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp + src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.cpp + src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.cpp + src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanFloat4CL.cpp + src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.cpp + src/Bullet3OpenCL/ParallelPrimitives/b3BoundSearchCL.cpp + src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.cpp + src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp + src/Bullet3OpenCL/RigidBody/b3Solver.cpp + src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp + src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp + src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp + src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp + src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp + src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp + src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp + src/LinearMath/TaskScheduler/btTaskScheduler.cpp + src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp + src/LinearMath/btPolarDecomposition.cpp + src/LinearMath/btConvexHull.cpp + src/LinearMath/btSerializer.cpp + src/LinearMath/btAlignedAllocator.cpp + src/LinearMath/btQuickprof.cpp + src/LinearMath/btConvexHullComputer.cpp + src/LinearMath/btSerializer64.cpp + src/LinearMath/btGeometryUtil.cpp + src/LinearMath/btThreads.cpp + src/LinearMath/btVector3.cpp + src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp + src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp + src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp + src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp + src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp + src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp + src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp - src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp src/BulletCollision/BroadphaseCollision/btDbvt.cpp - src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp - src/BulletCollision/BroadphaseCollision/btDispatcher.cpp - src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp - src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp + src/BulletCollision/BroadphaseCollision/btDispatcher.cpp + src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp + src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp + src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp - src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp - src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp - src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp - src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp - src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp - src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp - src/BulletCollision/CollisionDispatch/btCollisionObject.cpp - src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp + src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp - src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp + src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp + src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp + src/BulletCollision/CollisionDispatch/btUnionFind.cpp + src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp - src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp - src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp - src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp + src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp - src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp + src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp + src/BulletCollision/CollisionDispatch/btCollisionObject.cpp + src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp src/BulletCollision/CollisionDispatch/btGhostObject.cpp + src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp + src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp + src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp + src/BulletCollision/CollisionDispatch/btManifoldResult.cpp + src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp + src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp + src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp + src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp - src/BulletCollision/CollisionDispatch/btManifoldResult.cpp - src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp - src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp - src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp - src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp - src/BulletCollision/CollisionDispatch/btUnionFind.cpp - src/BulletCollision/CollisionShapes/btBox2dShape.cpp - src/BulletCollision/CollisionShapes/btBoxShape.cpp - src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp - src/BulletCollision/CollisionShapes/btCapsuleShape.cpp - src/BulletCollision/CollisionShapes/btCollisionShape.cpp - src/BulletCollision/CollisionShapes/btCompoundShape.cpp - src/BulletCollision/CollisionShapes/btConcaveShape.cpp - src/BulletCollision/CollisionShapes/btConeShape.cpp + src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp + src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp + src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp + src/BulletCollision/CollisionShapes/btTriangleCallback.cpp + src/BulletCollision/CollisionShapes/btShapeHull.cpp src/BulletCollision/CollisionShapes/btConvex2dShape.cpp - src/BulletCollision/CollisionShapes/btConvexHullShape.cpp - src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp - src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp - src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp src/BulletCollision/CollisionShapes/btConvexShape.cpp - src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp - src/BulletCollision/CollisionShapes/btCylinderShape.cpp - src/BulletCollision/CollisionShapes/btEmptyShape.cpp - src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp - src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp - src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp + src/BulletCollision/CollisionShapes/btConvexHullShape.cpp + src/BulletCollision/CollisionShapes/btCapsuleShape.cpp src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp + src/BulletCollision/CollisionShapes/btMiniSDF.cpp + src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp + src/BulletCollision/CollisionShapes/btEmptyShape.cpp + src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp - src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp + src/BulletCollision/CollisionShapes/btCompoundShape.cpp + src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp + src/BulletCollision/CollisionShapes/btCollisionShape.cpp src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp - src/BulletCollision/CollisionShapes/btShapeHull.cpp - src/BulletCollision/CollisionShapes/btSphereShape.cpp + src/BulletCollision/CollisionShapes/btCylinderShape.cpp + src/BulletCollision/CollisionShapes/btConcaveShape.cpp src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp - src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp - src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp + src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp + src/BulletCollision/CollisionShapes/btBoxShape.cpp src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp - src/BulletCollision/CollisionShapes/btTriangleCallback.cpp + src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp + src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp - src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp - src/BulletCollision/CollisionShapes/btTriangleMesh.cpp - src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp + src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp + src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp + src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp + src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp + src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp - src/BulletCollision/Gimpact/btContactProcessing.cpp - src/BulletCollision/Gimpact/btGImpactBvh.cpp - src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp - src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp + src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp + src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp + src/BulletCollision/CollisionShapes/btSphereShape.cpp + src/BulletCollision/CollisionShapes/btBox2dShape.cpp + src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp + src/BulletCollision/CollisionShapes/btConeShape.cpp + src/BulletCollision/CollisionShapes/btTriangleMesh.cpp + src/BulletCollision/Gimpact/gim_contact.cpp + src/BulletCollision/Gimpact/gim_tri_collision.cpp src/BulletCollision/Gimpact/btGImpactShape.cpp - src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp + src/BulletCollision/Gimpact/btContactProcessing.cpp src/BulletCollision/Gimpact/btTriangleShapeEx.cpp + src/BulletCollision/Gimpact/btGImpactBvh.cpp src/BulletCollision/Gimpact/gim_box_set.cpp - src/BulletCollision/Gimpact/gim_contact.cpp + src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp + src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp + src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp src/BulletCollision/Gimpact/gim_memory.cpp - src/BulletCollision/Gimpact/gim_tri_collision.cpp - src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp - src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp - src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp + src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp - src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp + src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp - src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp - src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp + src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp + src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp - src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp - - src/BulletDynamics/Character/btKinematicCharacterController.cpp - src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp - src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp - src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp - src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp - src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp - src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp - src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp - src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp - src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp - src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp - src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp - src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp - src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp - src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp - src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp - src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp + src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp + src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp + src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp src/BulletDynamics/Dynamics/Bullet-C-API.cpp - src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp - src/BulletDynamics/Dynamics/btRigidBody.cpp - src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp - src/BulletDynamics/Featherstone/btMultiBody.cpp - src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp - src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp - src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp - src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp - src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp - src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp - src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp - src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp - src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp - src/BulletDynamics/Vehicle/btRaycastVehicle.cpp - src/BulletDynamics/Vehicle/btWheelInfo.cpp - - src/BulletSoftBody/btDefaultSoftBodySolver.cpp - src/BulletSoftBody/btSoftBody.cpp - src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp - src/BulletSoftBody/btSoftBodyHelpers.cpp - src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp - src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp - src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp - src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp - - src/LinearMath/btAlignedAllocator.cpp - src/LinearMath/btConvexHull.cpp - src/LinearMath/btConvexHullComputer.cpp - src/LinearMath/btGeometryUtil.cpp - src/LinearMath/btPolarDecomposition.cpp - src/LinearMath/btQuickprof.cpp - src/LinearMath/btSerializer.cpp - src/LinearMath/btVector3.cpp - src/BulletCollision/BroadphaseCollision/btAxisSweep3.h - src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h - src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h - src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h - src/BulletCollision/BroadphaseCollision/btDbvt.h - src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h - src/BulletCollision/BroadphaseCollision/btDispatcher.h - src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h - src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h - src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h - src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h - src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h - src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h - src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h - src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h - src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h - src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h - src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h - src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h - src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h - src/BulletCollision/CollisionDispatch/btCollisionObject.h - src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h - src/BulletCollision/CollisionDispatch/btCollisionWorld.h - src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h - src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h - src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h - src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h - src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h - src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h + src/BulletDynamics/Featherstone/btMultiBodyLink.h + src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h + src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h + src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h + src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h + src/BulletDynamics/Featherstone/btMultiBodyConstraint.h + src/BulletDynamics/Featherstone/btMultiBody.h + src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h + src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h + src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h + src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h + src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h + src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h + src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h + src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h + src/BulletDynamics/Dynamics/btRigidBody.h + src/BulletDynamics/Dynamics/btActionInterface.h + src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h + src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h + src/BulletDynamics/Dynamics/btDynamicsWorld.h + src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h + src/BulletDynamics/Character/btKinematicCharacterController.h + src/BulletDynamics/Character/btCharacterControllerInterface.h + src/BulletDynamics/MLCPSolvers/btDantzigLCP.h + src/BulletDynamics/MLCPSolvers/btPATHSolver.h + src/BulletDynamics/MLCPSolvers/btMLCPSolver.h + src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h + src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h + src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h + src/BulletDynamics/MLCPSolvers/btDantzigSolver.h + src/BulletDynamics/MLCPSolvers/btLemkeSolver.h + src/BulletDynamics/Vehicle/btVehicleRaycaster.h + src/BulletDynamics/Vehicle/btRaycastVehicle.h + src/BulletDynamics/Vehicle/btWheelInfo.h + src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h + src/BulletDynamics/ConstraintSolver/btConstraintSolver.h + src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h + src/BulletDynamics/ConstraintSolver/btGearConstraint.h + src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h + src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h + src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h + src/BulletDynamics/ConstraintSolver/btHingeConstraint.h + src/BulletDynamics/ConstraintSolver/btTypedConstraint.h + src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h + src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h + src/BulletDynamics/ConstraintSolver/btContactConstraint.h + src/BulletDynamics/ConstraintSolver/btSolverConstraint.h + src/BulletDynamics/ConstraintSolver/btSliderConstraint.h + src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h + src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h + src/BulletDynamics/ConstraintSolver/btJacobianEntry.h + src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h + src/BulletDynamics/ConstraintSolver/btSolverBody.h + src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h + src/BulletDynamics/ConstraintSolver/btFixedConstraint.h + src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h + src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h + src/BulletSoftBody/btSoftBodyData.h + src/BulletSoftBody/btDefaultSoftBodySolver.h + src/BulletSoftBody/btSoftBodySolverVertexBuffer.h + src/BulletSoftBody/btSoftBodyHelpers.h + src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h + src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h + src/BulletSoftBody/btSoftRigidDynamicsWorld.h + src/BulletSoftBody/btSoftRigidDynamicsWorldMt.h + src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h + src/BulletSoftBody/btSparseSDF.h + src/BulletSoftBody/btSoftBody.h + src/BulletSoftBody/btSoftBodySolvers.h + src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h + src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h + src/BulletSoftBody/btSoftBodyInternals.h + src/btBulletCollisionCommon.h + src/Bullet3Geometry/b3ConvexHullComputer.h + src/Bullet3Geometry/b3GrahamScan2dConvexHull.h + src/Bullet3Geometry/b3GeometryUtil.h + src/Bullet3Geometry/b3AabbUtil.h + src/clew/clew.h + src/Bullet3Common/shared/b3Int4.h + src/Bullet3Common/shared/b3Quat.h + src/Bullet3Common/shared/b3Float4.h + src/Bullet3Common/shared/b3PlatformDefinitions.h + src/Bullet3Common/shared/b3Mat3x3.h + src/Bullet3Common/shared/b3Int2.h + src/Bullet3Common/b3Scalar.h + src/Bullet3Common/b3StackAlloc.h + src/Bullet3Common/b3Random.h + src/Bullet3Common/b3AlignedObjectArray.h + src/Bullet3Common/b3Matrix3x3.h + src/Bullet3Common/b3CommandLineArgs.h + src/Bullet3Common/b3ResizablePool.h + src/Bullet3Common/b3FileUtils.h + src/Bullet3Common/b3TransformUtil.h + src/Bullet3Common/b3Transform.h + src/Bullet3Common/b3Logging.h + src/Bullet3Common/b3Quaternion.h + src/Bullet3Common/b3QuadWord.h + src/Bullet3Common/b3AlignedAllocator.h + src/Bullet3Common/b3HashMap.h + src/Bullet3Common/b3PoolAllocator.h + src/Bullet3Common/b3MinMax.h + src/Bullet3Common/b3Vector3.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h + src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h + src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h + src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h + src/Bullet3Collision/NarrowPhaseCollision/b3Config.h + src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h + src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h + src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h + src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h + src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h + src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h + src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h + src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h + src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h + src/btBulletDynamicsCommon.h + src/Bullet3Serialize/Bullet2FileLoader/b3Defines.h + src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h + src/Bullet3Serialize/Bullet2FileLoader/b3File.h + src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.h + src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.h + src/Bullet3Serialize/Bullet2FileLoader/b3DNA.h + src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.h + src/Bullet3Serialize/Bullet2FileLoader/b3Common.h + src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphaseKernels.h + src/Bullet3OpenCL/BroadphaseCollision/kernels/sapKernels.h + src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvhKernels.h + src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.h + src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h + src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.h + src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.h + src/Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h + src/Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h + src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h + src/Bullet3OpenCL/Raycast/b3GpuRaycast.h + src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h + src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.h + src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h + src/Bullet3OpenCL/NarrowphaseCollision/kernels/satKernels.h + src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcaveKernels.h + src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h + src/Bullet3OpenCL/NarrowphaseCollision/kernels/mprKernels.h + src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.h + src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h + src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.h + src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h + src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h + src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.h + src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h + src/Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h + src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.h + src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h + src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h + src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h + src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.h + src/Bullet3OpenCL/ParallelPrimitives/b3BufferInfoCL.h + src/Bullet3OpenCL/ParallelPrimitives/kernels/RadixSort32KernelsCL.h + src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernelsCL.h + src/Bullet3OpenCL/ParallelPrimitives/kernels/PrefixScanKernelsFloat4CL.h + src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernelsCL.h + src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernelsCL.h + src/Bullet3OpenCL/ParallelPrimitives/kernels/PrefixScanKernelsCL.h + src/Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h + src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanFloat4CL.h + src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h + src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h + src/Bullet3OpenCL/ParallelPrimitives/b3BoundSearchCL.h + src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h + src/Bullet3OpenCL/Initialize/b3OpenCLUtils.h + src/Bullet3OpenCL/Initialize/b3OpenCLInclude.h + src/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h + src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h + src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h + src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h + src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h + src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h + src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h + src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h + src/Bullet3OpenCL/RigidBody/kernels/solveContact.h + src/Bullet3OpenCL/RigidBody/kernels/solveFriction.h + src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h + src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h + src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h + src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h + src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h + src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h + src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h + src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h + src/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h + src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h + src/Bullet3OpenCL/RigidBody/b3Solver.h + src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h + src/LinearMath/btMinMax.h + src/LinearMath/btQuaternion.h + src/LinearMath/btTransform.h + src/LinearMath/btStackAlloc.h + src/LinearMath/btPolarDecomposition.h + src/LinearMath/TaskScheduler/btThreadSupportInterface.h + src/LinearMath/btAabbUtil2.h + src/LinearMath/btDefaultMotionState.h + src/LinearMath/btSpatialAlgebra.h + src/LinearMath/btHashMap.h + src/LinearMath/btMotionState.h + src/LinearMath/btAlignedObjectArray.h + src/LinearMath/btConvexHull.h + src/LinearMath/btIDebugDraw.h + src/LinearMath/btList.h + src/LinearMath/btPoolAllocator.h + src/LinearMath/btCpuFeatureUtility.h + src/LinearMath/btMatrix3x3.h + src/LinearMath/btGrahamScan2dConvexHull.h + src/LinearMath/btGeometryUtil.h + src/LinearMath/btVector3.h + src/LinearMath/btTransformUtil.h + src/LinearMath/btRandom.h + src/LinearMath/btSerializer.h + src/LinearMath/btAlignedAllocator.h + src/LinearMath/btScalar.h + src/LinearMath/btQuadWord.h + src/LinearMath/btMatrixX.h + src/LinearMath/btQuickprof.h + src/LinearMath/btConvexHullComputer.h + src/LinearMath/btThreads.h + src/Bullet3Dynamics/shared/b3Inertia.h + src/Bullet3Dynamics/shared/b3ConvertConstraint4.h + src/Bullet3Dynamics/shared/b3IntegrateTransforms.h + src/Bullet3Dynamics/shared/b3ContactConstraint4.h + src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h + src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h + src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h + src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h + src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h + src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h + src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h + src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h + src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h + src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h + src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h + src/BulletCollision/BroadphaseCollision/btAxisSweep3.h + src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h + src/BulletCollision/BroadphaseCollision/btDbvt.h + src/BulletCollision/BroadphaseCollision/btDispatcher.h + src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h + src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h + src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h + src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h + src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h + src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h + src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h + src/BulletCollision/CollisionDispatch/btManifoldResult.h src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h - src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h - src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h + src/BulletCollision/CollisionDispatch/btUnionFind.h + src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h src/BulletCollision/CollisionDispatch/btGhostObject.h - src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h + src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h + src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h + src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h + src/BulletCollision/CollisionDispatch/btCollisionObject.h src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h - src/BulletCollision/CollisionDispatch/btManifoldResult.h - src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h - src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h - src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h + src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h + src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h + src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h - src/BulletCollision/CollisionDispatch/btUnionFind.h - src/BulletCollision/CollisionShapes/btBox2dShape.h - src/BulletCollision/CollisionShapes/btBoxShape.h + src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h + src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h + src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h + src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h + src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h + src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h + src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h + src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h + src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h + src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h + src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h + src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h + src/BulletCollision/CollisionDispatch/btCollisionWorld.h + src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h + src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h + src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h + src/BulletCollision/CollisionShapes/btTriangleMeshShape.h + src/BulletCollision/CollisionShapes/btStaticPlaneShape.h + src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h - src/BulletCollision/CollisionShapes/btCapsuleShape.h + src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h src/BulletCollision/CollisionShapes/btCollisionMargin.h - src/BulletCollision/CollisionShapes/btCollisionShape.h - src/BulletCollision/CollisionShapes/btCompoundShape.h - src/BulletCollision/CollisionShapes/btConcaveShape.h - src/BulletCollision/CollisionShapes/btConeShape.h - src/BulletCollision/CollisionShapes/btConvex2dShape.h - src/BulletCollision/CollisionShapes/btConvexHullShape.h - src/BulletCollision/CollisionShapes/btConvexInternalShape.h - src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h - src/BulletCollision/CollisionShapes/btConvexPolyhedron.h + src/BulletCollision/CollisionShapes/btSphereShape.h + src/BulletCollision/CollisionShapes/btTriangleShape.h + src/BulletCollision/CollisionShapes/btCapsuleShape.h src/BulletCollision/CollisionShapes/btConvexShape.h - src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h + src/BulletCollision/CollisionShapes/btBoxShape.h + src/BulletCollision/CollisionShapes/btTriangleCallback.h + src/BulletCollision/CollisionShapes/btConvexPolyhedron.h + src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h + src/BulletCollision/CollisionShapes/btConeShape.h src/BulletCollision/CollisionShapes/btCylinderShape.h - src/BulletCollision/CollisionShapes/btEmptyShape.h - src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h - src/BulletCollision/CollisionShapes/btMaterial.h src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h + src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h + src/BulletCollision/CollisionShapes/btTriangleInfoMap.h + src/BulletCollision/CollisionShapes/btEmptyShape.h + src/BulletCollision/CollisionShapes/btSdfCollisionShape.h + src/BulletCollision/CollisionShapes/btTriangleMesh.h + src/BulletCollision/CollisionShapes/btUniformScalingShape.h + src/BulletCollision/CollisionShapes/btTriangleBuffer.h + src/BulletCollision/CollisionShapes/btConcaveShape.h + src/BulletCollision/CollisionShapes/btBox2dShape.h + src/BulletCollision/CollisionShapes/btCollisionShape.h + src/BulletCollision/CollisionShapes/btMiniSDF.h + src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h + src/BulletCollision/CollisionShapes/btConvexInternalShape.h + src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h + src/BulletCollision/CollisionShapes/btStridingMeshInterface.h src/BulletCollision/CollisionShapes/btMultiSphereShape.h src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h src/BulletCollision/CollisionShapes/btOptimizedBvh.h - src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h - src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h - src/BulletCollision/CollisionShapes/btShapeHull.h - src/BulletCollision/CollisionShapes/btSphereShape.h - src/BulletCollision/CollisionShapes/btStaticPlaneShape.h - src/BulletCollision/CollisionShapes/btStridingMeshInterface.h src/BulletCollision/CollisionShapes/btTetrahedronShape.h - src/BulletCollision/CollisionShapes/btTriangleBuffer.h - src/BulletCollision/CollisionShapes/btTriangleCallback.h + src/BulletCollision/CollisionShapes/btShapeHull.h src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h - src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h - src/BulletCollision/CollisionShapes/btTriangleInfoMap.h - src/BulletCollision/CollisionShapes/btTriangleMesh.h - src/BulletCollision/CollisionShapes/btTriangleMeshShape.h - src/BulletCollision/CollisionShapes/btTriangleShape.h - src/BulletCollision/CollisionShapes/btUniformScalingShape.h - src/BulletCollision/Gimpact/btBoxCollision.h - src/BulletCollision/Gimpact/btClipPolygon.h - src/BulletCollision/Gimpact/btCompoundFromGimpact.h - src/BulletCollision/Gimpact/btContactProcessing.h - src/BulletCollision/Gimpact/btGImpactBvh.h - src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h - src/BulletCollision/Gimpact/btGImpactMassUtil.h - src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h - src/BulletCollision/Gimpact/btGImpactShape.h - src/BulletCollision/Gimpact/btGenericPoolAllocator.h + src/BulletCollision/CollisionShapes/btConvexHullShape.h + src/BulletCollision/CollisionShapes/btCompoundShape.h + src/BulletCollision/CollisionShapes/btConvex2dShape.h + src/BulletCollision/CollisionShapes/btMaterial.h src/BulletCollision/Gimpact/btGeometryOperations.h - src/BulletCollision/Gimpact/btQuantization.h - src/BulletCollision/Gimpact/btTriangleShapeEx.h + src/BulletCollision/Gimpact/gim_geometry.h + src/BulletCollision/Gimpact/gim_math.h + src/BulletCollision/Gimpact/gim_tri_collision.h + src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h + src/BulletCollision/Gimpact/gim_hash_table.h src/BulletCollision/Gimpact/gim_array.h + src/BulletCollision/Gimpact/btBoxCollision.h + src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h + src/BulletCollision/Gimpact/gim_box_set.h + src/BulletCollision/Gimpact/gim_contact.h src/BulletCollision/Gimpact/gim_basic_geometry_operations.h + src/BulletCollision/Gimpact/btGImpactShape.h + src/BulletCollision/Gimpact/btGenericPoolAllocator.h src/BulletCollision/Gimpact/gim_bitset.h - src/BulletCollision/Gimpact/gim_box_collision.h - src/BulletCollision/Gimpact/gim_box_set.h src/BulletCollision/Gimpact/gim_clip_polygon.h - src/BulletCollision/Gimpact/gim_contact.h + src/BulletCollision/Gimpact/btGImpactMassUtil.h + src/BulletCollision/Gimpact/btCompoundFromGimpact.h + src/BulletCollision/Gimpact/btContactProcessingStructs.h src/BulletCollision/Gimpact/gim_geom_types.h - src/BulletCollision/Gimpact/gim_geometry.h - src/BulletCollision/Gimpact/gim_hash_table.h - src/BulletCollision/Gimpact/gim_linear_math.h - src/BulletCollision/Gimpact/gim_math.h - src/BulletCollision/Gimpact/gim_memory.h + src/BulletCollision/Gimpact/gim_box_collision.h + src/BulletCollision/Gimpact/btGImpactBvh.h src/BulletCollision/Gimpact/gim_radixsort.h - src/BulletCollision/Gimpact/gim_tri_collision.h - src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h - src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h - src/BulletCollision/NarrowPhaseCollision/btConvexCast.h - src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h - src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h - src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h + src/BulletCollision/Gimpact/btQuantization.h + src/BulletCollision/Gimpact/btGImpactBvhStructs.h + src/BulletCollision/Gimpact/btClipPolygon.h + src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h + src/BulletCollision/Gimpact/gim_memory.h + src/BulletCollision/Gimpact/btTriangleShapeEx.h + src/BulletCollision/Gimpact/btContactProcessing.h + src/BulletCollision/Gimpact/gim_linear_math.h src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h - src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h - src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h - src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h - src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h - src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h + src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h + src/BulletCollision/NarrowPhaseCollision/btConvexCast.h src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h + src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h - src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h - src/BulletCollision/NarrowPhaseCollision/btPointCollector.h + src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h - src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h + src/BulletCollision/NarrowPhaseCollision/btPointCollector.h src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h - src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h + src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h - - src/BulletDynamics/Character/btCharacterControllerInterface.h - src/BulletDynamics/Character/btKinematicCharacterController.h - src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h - src/BulletDynamics/ConstraintSolver/btConstraintSolver.h - src/BulletDynamics/ConstraintSolver/btContactConstraint.h - src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h - src/BulletDynamics/ConstraintSolver/btFixedConstraint.h - src/BulletDynamics/ConstraintSolver/btGearConstraint.h - src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h - src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h - src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h - src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h - src/BulletDynamics/ConstraintSolver/btHingeConstraint.h - src/BulletDynamics/ConstraintSolver/btJacobianEntry.h - src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h - src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h - src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h - src/BulletDynamics/ConstraintSolver/btSliderConstraint.h - src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h - src/BulletDynamics/ConstraintSolver/btSolverBody.h - src/BulletDynamics/ConstraintSolver/btSolverConstraint.h - src/BulletDynamics/ConstraintSolver/btTypedConstraint.h - src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h - src/BulletDynamics/Dynamics/btActionInterface.h - src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h - src/BulletDynamics/Dynamics/btDynamicsWorld.h - src/BulletDynamics/Dynamics/btRigidBody.h - src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h - src/BulletDynamics/Featherstone/btMultiBody.h - src/BulletDynamics/Featherstone/btMultiBodyConstraint.h - src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h - src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h - src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h - src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h - src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h - src/BulletDynamics/Featherstone/btMultiBodyLink.h - src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h - src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h - src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h - src/BulletDynamics/MLCPSolvers/btDantzigLCP.h - src/BulletDynamics/MLCPSolvers/btDantzigSolver.h - src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h - src/BulletDynamics/MLCPSolvers/btLemkeSolver.h - src/BulletDynamics/MLCPSolvers/btMLCPSolver.h - src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h - src/BulletDynamics/MLCPSolvers/btPATHSolver.h - src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h - src/BulletDynamics/Vehicle/btRaycastVehicle.h - src/BulletDynamics/Vehicle/btVehicleRaycaster.h - src/BulletDynamics/Vehicle/btWheelInfo.h - - src/BulletSoftBody/btDefaultSoftBodySolver.h - src/BulletSoftBody/btSoftBody.h - src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h - src/BulletSoftBody/btSoftBodyData.h - src/BulletSoftBody/btSoftBodyHelpers.h - src/BulletSoftBody/btSoftBodyInternals.h - src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h - src/BulletSoftBody/btSoftBodySolverVertexBuffer.h - src/BulletSoftBody/btSoftBodySolvers.h - src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h - src/BulletSoftBody/btSoftRigidDynamicsWorld.h - src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h - src/BulletSoftBody/btSparseSDF.h - - src/LinearMath/btAabbUtil2.h - src/LinearMath/btAlignedAllocator.h - src/LinearMath/btAlignedObjectArray.h - src/LinearMath/btConvexHull.h - src/LinearMath/btConvexHullComputer.h - src/LinearMath/btCpuFeatureUtility.h - src/LinearMath/btDefaultMotionState.h - src/LinearMath/btGeometryUtil.h - src/LinearMath/btGrahamScan2dConvexHull.h - src/LinearMath/btHashMap.h - src/LinearMath/btIDebugDraw.h - src/LinearMath/btList.h - src/LinearMath/btMatrix3x3.h - src/LinearMath/btMatrixX.h - src/LinearMath/btMinMax.h - src/LinearMath/btMotionState.h - src/LinearMath/btPolarDecomposition.h - src/LinearMath/btPoolAllocator.h - src/LinearMath/btQuadWord.h - src/LinearMath/btQuaternion.h - src/LinearMath/btQuickprof.h - src/LinearMath/btRandom.h - src/LinearMath/btScalar.h - src/LinearMath/btSerializer.h - src/LinearMath/btSpatialAlgebra.h - src/LinearMath/btStackAlloc.h - src/LinearMath/btTransform.h - src/LinearMath/btTransformUtil.h - src/LinearMath/btVector3.h - - src/btBulletCollisionCommon.h - src/btBulletDynamicsCommon.h + src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h + src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h + src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h + src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h + src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h + src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h + src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h + src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h + src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h src/Bullet-C-Api.h ) +add_definitions( + -DBT_THREADSAFE + -DBT_USE_TBB + -DB3_USE_CLEW +) + if(CMAKE_COMPILER_IS_GNUCXX) # needed for gcc 4.6+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fpermissive") diff --git a/extern/bullet/LICENSE.txt b/extern/bullet/LICENSE.txt new file mode 100644 index 000000000000..319c84e349f6 --- /dev/null +++ b/extern/bullet/LICENSE.txt @@ -0,0 +1,15 @@ + +The files in this repository are licensed under the zlib license, except for the files under 'Extras' and examples/ThirdPartyLibs. + +Bullet Continuous Collision Detection and Physics Library +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. diff --git a/extern/bullet/README.md b/extern/bullet/README.md new file mode 100644 index 000000000000..3bbc2e2c7b89 --- /dev/null +++ b/extern/bullet/README.md @@ -0,0 +1,119 @@ + +[![Travis Build Status](https://api.travis-ci.org/bulletphysics/bullet3.png?branch=master)](https://travis-ci.org/bulletphysics/bullet3) +[![Appveyor Build status](https://ci.appveyor.com/api/projects/status/6sly9uxajr6xsstq)](https://ci.appveyor.com/project/erwincoumans/bullet3) + +# Bullet Physics SDK + +This is the official C++ source code repository of the Bullet Physics SDK: real-time collision detection and multi-physics simulation for VR, games, visual effects, robotics, machine learning etc. + +New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR. Use pip install pybullet and see [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3). + +The Bullet 2 API will stay default and up-to-date while slowly moving to a new API. +The steps towards a new API is in a nutshell: + +1. The old Bullet2 demos are being merged into the examples/ExampleBrowser +2. A new physics-engine agnostic C-API is created, see examples/SharedMemory/PhysicsClientC_API.h +3. Python bindings in pybullet are on top of this C-API, see examples/pybullet +4. A Virtual Reality sandbox using openvr for HTC Vive and Oculus Rift is available +5. The OpenCL examples in the ExampleBrowser can be enabled using --enable_experimental_opencl + +You can still use svn or svn externals using the github git repository: use svn co https://github.com/bulletphysics/bullet3/trunk + +## Requirements for Bullet 2 + +A C++ compiler for C++ 2003. The library is tested on Windows, Linux, Mac OSX, iOS, Android, +but should likely work on any platform with C++ compiler. +Some optional demos require OpenGL 2 or OpenGL 3, there are some non-graphical demos and unit tests too. + +## Contributors and Coding Style information + +https://docs.google.com/document/d/1u9vyzPtrVoVhYqQOGNWUgjRbfwfCdIts_NzmvgiJ144/edit + +## Requirements for experimental OpenCL GPGPU support + +The entire collision detection and rigid body dynamics can be executed on the GPU. + +A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better. +We succesfully tested the software under Windows, Linux and Mac OSX. +The software currently doesn't work on OpenCL CPU devices. It might run +on a laptop GPU but performance will not likely be very good. Note that +often an OpenCL drivers fails to compile a kernel. Some unit tests exist to +track down the issue, but more work is required to cover all OpenCL kernels. + +## License + +All source code files are licensed under the permissive zlib license +(http://opensource.org/licenses/Zlib) unless marked differently in a particular folder/file. + +## Build instructions for Bullet using premake. You can also use cmake instead. + +**Windows** + +Click on build_visual_studio_vr_pybullet_double.bat and open build3/vs2010/0MySolution.sln +When asked, convert the projects to a newer version of Visual Studio. +If you installed Python in the C:\ root directory, the batch file should find it automatically. +Otherwise, edit this batch file to choose where Python include/lib directories are located. + +**Windows Virtual Reality sandbox for HTC Vive and Oculus Rift** + +Build and run the App_SharedMemoryPhysics_VR project, preferably in Release/optimized build. +You can connect from Python pybullet to the sandbox using: + +``` +import pybullet as p +p.connect(p.SHARED_MEMORY) #or (p.TCP, "localhost", 6667) or (p.UDP, "192.168.86.10",1234) +``` + +**Linux and Mac OSX gnu make** + +Make sure cmake is installed (sudo apt-get install cmake, brew install cmake, or https://cmake.org) + +In a terminal type: + + ./build_cmake_pybullet_double.sh + +This script will invoke cmake and build in the build_cmake directory. You can find pybullet in Bullet/examples/pybullet. +The BulletExampleBrowser binary will be in Bullet/examples/ExampleBrowser. + +You can also build Bullet using premake. There are premake executables in the build3 folder. +Depending on your system (Linux 32bit, 64bit or Mac OSX) use one of the following lines +Using premake: +``` + cd build3 + ./premake4_linux --double gmake + ./premake4_linux64 --double gmake + ./premake4_osx --double --enable_pybullet gmake +``` +Then +``` + cd gmake + make +``` + +Note that on Linux, you need to use cmake to build pybullet, since the compiler has issues of mixing shared and static libraries. + +**Mac OSX Xcode** + +Click on build3/xcode4.command or in a terminal window execute + + ./premake_osx xcode4 + +## Usage + +The App_ExampleBrowser executables will be located in the bin folder. +You can just run it though a terminal/command prompt, or by clicking it. + + +``` +[--start_demo_name="Demo Name"] Start with a selected demo +[--mp4=moviename.mp4] Create a mp4 movie of the window, requires ffmpeg installed +[--mouse_move_multiplier=0.400000] Set the mouse move sensitivity +[--mouse_wheel_multiplier=0.01] Set the mouse wheel sensitivity +[--background_color_red= 0.9] Set the red component for background color. Same for green and blue +[--fixed_timestep= 0.0] Use either a real-time delta time (0.0) or a fixed step size (0.016666) +``` + +You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls. +Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app. + +Check out the docs folder and the Bullet physics forums for further information. diff --git a/extern/bullet/VERSION b/extern/bullet/VERSION new file mode 100644 index 000000000000..663ae72bcc57 --- /dev/null +++ b/extern/bullet/VERSION @@ -0,0 +1 @@ +2.88 diff --git a/extern/bullet2/patches/blender.patch b/extern/bullet/patches/blender.patch similarity index 65% rename from extern/bullet2/patches/blender.patch rename to extern/bullet/patches/blender.patch index cb3bf2ba38a0..5dfc704003d5 100644 --- a/extern/bullet2/patches/blender.patch +++ b/extern/bullet/patches/blender.patch @@ -1,6 +1,6 @@ -diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h ---- a/extern/bullet2/src/LinearMath/btScalar.h -+++ b/extern/bullet2/src/LinearMath/btScalar.h +diff --git a/extern/bullet/src/LinearMath/btScalar.h b/extern/bullet/src/LinearMath/btScalar.h +--- a/extern/bullet/src/LinearMath/btScalar.h ++++ b/extern/bullet/src/LinearMath/btScalar.h @@ -16,6 +16,9 @@ #ifndef BT_SCALAR_H @@ -29,10 +29,10 @@ diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/Linea #include #define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak(); }} #else//_MSC_VER -diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +diff --git a/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index be9eca6..ec40c96 100644 ---- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h -+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +--- a/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h ++++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -15,7 +15,7 @@ subject to the following restrictions: @@ -42,10 +42,10 @@ index be9eca6..ec40c96 100644 * * @section intro_sec Introduction * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ). -diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp +diff --git a/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp index 36dd043..57eb817 100644 ---- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp -+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp +--- a/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp ++++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp @@ -579,14 +579,10 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData; btCompoundShape* compoundShape = createCompoundShape(); @@ -61,10 +61,10 @@ index 36dd043..57eb817 100644 btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape; btCollisionShape* childShape = convertCollisionShape(cd); -diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp +diff --git a/extern/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/extern/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp index 57fc119..31faf1d 100644 ---- a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp -+++ b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp +--- a/extern/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp ++++ b/extern/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -29,14 +29,11 @@ subject to the following restrictions: static btVector3 getNormalizedVector(const btVector3& v) @@ -84,10 +84,10 @@ index 57fc119..31faf1d 100644 return n; } -diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h +diff --git a/extern/bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h index 27ccefe..8e4456e 100644 ---- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h -+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h +--- a/extern/bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h ++++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -37,8 +37,13 @@ struct btSimdScalar { @@ -103,10 +103,10 @@ index 27ccefe..8e4456e 100644 :m_vec128 (_mm_set1_ps(fl)) { } -diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp +diff --git a/extern/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp index 5d62da7..fcd312e 100644 ---- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp -+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp +--- a/extern/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp ++++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -28,7 +28,6 @@ #include "btMultiBodyJointFeedback.h" #include "LinearMath/btTransformUtil.h" @@ -123,10 +123,10 @@ index 5d62da7..fcd312e 100644 extern bool gDisableDeactivation; if (!m_canSleep || gDisableDeactivation) { -diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +diff --git a/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 8a034b3..4f66b20 100644 ---- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp -+++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +--- a/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp ++++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -809,7 +809,6 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS } #endif @@ -135,10 +135,10 @@ index 8a034b3..4f66b20 100644 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime) { #if 1 -diff --git a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h +diff --git a/extern/bullet/src/BulletSoftBody/btSparseSDF.h b/extern/bullet/src/BulletSoftBody/btSparseSDF.h index bcf0c79..8992ddb 100644 ---- a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h -+++ b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h +--- a/extern/bullet/src/BulletSoftBody/btSparseSDF.h ++++ b/extern/bullet/src/BulletSoftBody/btSparseSDF.h @@ -185,7 +185,6 @@ struct btSparseSdf { ++nprobes; @@ -147,10 +147,10 @@ index bcf0c79..8992ddb 100644 if (ncells>m_clampCells) { static int numResets=0; -diff --git a/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp b/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp +diff --git a/extern/bullet/src/LinearMath/btConvexHullComputer.cpp b/extern/bullet/src/LinearMath/btConvexHullComputer.cpp index d58ac95..3fd77df 100644 ---- a/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp -+++ b/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp +--- a/extern/bullet/src/LinearMath/btConvexHullComputer.cpp ++++ b/extern/bullet/src/LinearMath/btConvexHullComputer.cpp @@ -2665,6 +2665,7 @@ btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, in } @@ -167,10 +167,10 @@ index d58ac95..3fd77df 100644 btConvexHullInternal::Edge* firstEdge = v->edges; if (firstEdge) { -diff --git a/extern/bullet2/src/LinearMath/btConvexHullComputer.h b/extern/bullet2/src/LinearMath/btConvexHullComputer.h +diff --git a/extern/bullet/src/LinearMath/btConvexHullComputer.h b/extern/bullet/src/LinearMath/btConvexHullComputer.h index 7240ac4..6871ce8 100644 ---- a/extern/bullet2/src/LinearMath/btConvexHullComputer.h -+++ b/extern/bullet2/src/LinearMath/btConvexHullComputer.h +--- a/extern/bullet/src/LinearMath/btConvexHullComputer.h ++++ b/extern/bullet/src/LinearMath/btConvexHullComputer.h @@ -67,6 +67,7 @@ class btConvexHullComputer // Vertices of the output hull @@ -179,10 +179,10 @@ index 7240ac4..6871ce8 100644 // Edges of the output hull btAlignedObjectArray edges; -diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +diff --git a/extern/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 0623e35..02ea503 100644 ---- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp -+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +--- a/extern/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp ++++ b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -13,9 +13,9 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ @@ -196,10 +196,10 @@ index 0623e35..02ea503 100644 #include "btConvexHullShape.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" -diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp +diff --git a/extern/bullet/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexShape.cpp index b56d729..88018b4 100644 ---- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp -+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp +--- a/extern/bullet/src/BulletCollision/CollisionShapes/btConvexShape.cpp ++++ b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -13,9 +13,9 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ @@ -213,10 +213,10 @@ index b56d729..88018b4 100644 #include "btConvexShape.h" #include "btTriangleShape.h" -diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +diff --git a/extern/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index a7362ea..6abfdff 100644 ---- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp -+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +--- a/extern/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp ++++ b/extern/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -13,9 +13,9 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ @@ -230,10 +230,10 @@ index a7362ea..6abfdff 100644 #include "btMultiSphereShape.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" -diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +diff --git a/extern/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 4854f37..9095c59 100644 ---- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp -+++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +--- a/extern/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp ++++ b/extern/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -12,9 +12,9 @@ subject to the following restrictions: 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. @@ -247,10 +247,10 @@ index 4854f37..9095c59 100644 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" #include "btConvexPolyhedron.h" -diff --git a/extern/bullet2/src/LinearMath/btVector3.cpp b/extern/bullet2/src/LinearMath/btVector3.cpp +diff --git a/extern/bullet/src/LinearMath/btVector3.cpp b/extern/bullet/src/LinearMath/btVector3.cpp index e05bdcc..dbcf2b6 100644 ---- a/extern/bullet2/src/LinearMath/btVector3.cpp -+++ b/extern/bullet2/src/LinearMath/btVector3.cpp +--- a/extern/bullet/src/LinearMath/btVector3.cpp ++++ b/extern/bullet/src/LinearMath/btVector3.cpp @@ -15,9 +15,9 @@ This source version has been altered. */ @@ -264,10 +264,10 @@ index e05bdcc..dbcf2b6 100644 #include "btVector3.h" -diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +diff --git a/extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp index e0e8bc7..a788268 100644 ---- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp -+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +--- a/extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp ++++ b/extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -425,50 +425,38 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform) } @@ -340,10 +340,10 @@ index e0e8bc7..a788268 100644 } int btRigidBody::calculateSerializeBufferSize() const -diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +diff --git a/extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.h index 1d177db..c2f8c5d 100644 ---- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h -+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +--- a/extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.h ++++ b/extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.h @@ -509,6 +509,8 @@ public: return (getBroadphaseProxy() != 0); } diff --git a/extern/bullet2/src/Bullet-C-Api.h b/extern/bullet/src/Bullet-C-Api.h similarity index 100% rename from extern/bullet2/src/Bullet-C-Api.h rename to extern/bullet/src/Bullet-C-Api.h diff --git a/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h new file mode 100644 index 000000000000..1bc56cf80a5a --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h @@ -0,0 +1,40 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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. +*/ + +#ifndef B3_BROADPHASE_CALLBACK_H +#define B3_BROADPHASE_CALLBACK_H + +#include "Bullet3Common/b3Vector3.h" +struct b3BroadphaseProxy; + + +struct b3BroadphaseAabbCallback +{ + virtual ~b3BroadphaseAabbCallback() {} + virtual bool process(const b3BroadphaseProxy* proxy) = 0; +}; + + +struct b3BroadphaseRayCallback : public b3BroadphaseAabbCallback +{ + ///added some cached data to accelerate ray-AABB tests + b3Vector3 m_rayDirectionInverse; + unsigned int m_signs[3]; + b3Scalar m_lambda_max; + + virtual ~b3BroadphaseRayCallback() {} +}; + +#endif //B3_BROADPHASE_CALLBACK_H diff --git a/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp new file mode 100644 index 000000000000..0f04efe3311c --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp @@ -0,0 +1,1325 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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. +*/ +///b3DynamicBvh implementation by Nathanael Presson + +#include "b3DynamicBvh.h" + +// +typedef b3AlignedObjectArray b3NodeArray; +typedef b3AlignedObjectArray b3ConstNodeArray; + +// +struct b3DbvtNodeEnumerator : b3DynamicBvh::ICollide +{ + b3ConstNodeArray nodes; + void Process(const b3DbvtNode* n) { nodes.push_back(n); } +}; + +// +static B3_DBVT_INLINE int b3IndexOf(const b3DbvtNode* node) +{ + return(node->parent->childs[1]==node); +} + +// +static B3_DBVT_INLINE b3DbvtVolume b3Merge( const b3DbvtVolume& a, + const b3DbvtVolume& b) +{ +#if (B3_DBVT_MERGE_IMPL==B3_DBVT_IMPL_SSE) + B3_ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtAabbMm)]); + b3DbvtVolume& res=*(b3DbvtVolume*)locals; +#else + b3DbvtVolume res; +#endif + b3Merge(a,b,res); + return(res); +} + +// volume+edge lengths +static B3_DBVT_INLINE b3Scalar b3Size(const b3DbvtVolume& a) +{ + const b3Vector3 edges=a.Lengths(); + return( edges.x*edges.y*edges.z+ + edges.x+edges.y+edges.z); +} + +// +static void b3GetMaxDepth(const b3DbvtNode* node,int depth,int& maxdepth) +{ + if(node->isinternal()) + { + b3GetMaxDepth(node->childs[0],depth+1,maxdepth); + b3GetMaxDepth(node->childs[1],depth+1,maxdepth); + } else maxdepth=b3Max(maxdepth,depth); +} + +// +static B3_DBVT_INLINE void b3DeleteNode( b3DynamicBvh* pdbvt, + b3DbvtNode* node) +{ + b3AlignedFree(pdbvt->m_free); + pdbvt->m_free=node; +} + +// +static void b3RecurseDeleteNode( b3DynamicBvh* pdbvt, + b3DbvtNode* node) +{ + if(!node->isleaf()) + { + b3RecurseDeleteNode(pdbvt,node->childs[0]); + b3RecurseDeleteNode(pdbvt,node->childs[1]); + } + if(node==pdbvt->m_root) pdbvt->m_root=0; + b3DeleteNode(pdbvt,node); +} + +// +static B3_DBVT_INLINE b3DbvtNode* b3CreateNode( b3DynamicBvh* pdbvt, + b3DbvtNode* parent, + void* data) +{ + b3DbvtNode* node; + if(pdbvt->m_free) + { node=pdbvt->m_free;pdbvt->m_free=0; } + else + { node=new(b3AlignedAlloc(sizeof(b3DbvtNode),16)) b3DbvtNode(); } + node->parent = parent; + node->data = data; + node->childs[1] = 0; + return(node); +} + +// +static B3_DBVT_INLINE b3DbvtNode* b3CreateNode( b3DynamicBvh* pdbvt, + b3DbvtNode* parent, + const b3DbvtVolume& volume, + void* data) +{ + b3DbvtNode* node=b3CreateNode(pdbvt,parent,data); + node->volume=volume; + return(node); +} + +// +static B3_DBVT_INLINE b3DbvtNode* b3CreateNode( b3DynamicBvh* pdbvt, + b3DbvtNode* parent, + const b3DbvtVolume& volume0, + const b3DbvtVolume& volume1, + void* data) +{ + b3DbvtNode* node=b3CreateNode(pdbvt,parent,data); + b3Merge(volume0,volume1,node->volume); + return(node); +} + +// +static void b3InsertLeaf( b3DynamicBvh* pdbvt, + b3DbvtNode* root, + b3DbvtNode* leaf) +{ + if(!pdbvt->m_root) + { + pdbvt->m_root = leaf; + leaf->parent = 0; + } + else + { + if(!root->isleaf()) + { + do { + root=root->childs[b3Select( leaf->volume, + root->childs[0]->volume, + root->childs[1]->volume)]; + } while(!root->isleaf()); + } + b3DbvtNode* prev=root->parent; + b3DbvtNode* node=b3CreateNode(pdbvt,prev,leaf->volume,root->volume,0); + if(prev) + { + prev->childs[b3IndexOf(root)] = node; + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + do { + if(!prev->volume.Contain(node->volume)) + b3Merge(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 b3DbvtNode* b3RemoveLeaf( b3DynamicBvh* pdbvt, + b3DbvtNode* leaf) +{ + if(leaf==pdbvt->m_root) + { + pdbvt->m_root=0; + return(0); + } + else + { + b3DbvtNode* parent=leaf->parent; + b3DbvtNode* prev=parent->parent; + b3DbvtNode* sibling=parent->childs[1-b3IndexOf(leaf)]; + if(prev) + { + prev->childs[b3IndexOf(parent)]=sibling; + sibling->parent=prev; + b3DeleteNode(pdbvt,parent); + while(prev) + { + const b3DbvtVolume pb=prev->volume; + b3Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + if(b3NotEqual(pb,prev->volume)) + { + prev=prev->parent; + } else break; + } + return(prev?prev:pdbvt->m_root); + } + else + { + pdbvt->m_root=sibling; + sibling->parent=0; + b3DeleteNode(pdbvt,parent); + return(pdbvt->m_root); + } + } +} + +// +static void b3FetchLeaves(b3DynamicBvh* pdbvt, + b3DbvtNode* root, + b3NodeArray& leaves, + int depth=-1) +{ + if(root->isinternal()&&depth) + { + b3FetchLeaves(pdbvt,root->childs[0],leaves,depth-1); + b3FetchLeaves(pdbvt,root->childs[1],leaves,depth-1); + b3DeleteNode(pdbvt,root); + } + else + { + leaves.push_back(root); + } +} + +static bool b3LeftOfAxis( const b3DbvtNode* node, + const b3Vector3& org, + const b3Vector3& axis) +{ + return b3Dot(axis,node->volume.Center()-org) <= 0; +} + +// Partitions leaves such that leaves[0, n) are on the +// left of axis, and leaves[n, count) are on the right +// of axis. returns N. +static int b3Split( b3DbvtNode** leaves, + int count, + const b3Vector3& org, + const b3Vector3& axis) +{ + int begin=0; + int end=count; + for(;;) + { + while(begin!=end && b3LeftOfAxis(leaves[begin],org,axis)) + { + ++begin; + } + + if(begin==end) + { + break; + } + + while(begin!=end && !b3LeftOfAxis(leaves[end-1],org,axis)) + { + --end; + } + + if(begin==end) + { + break; + } + + // swap out of place nodes + --end; + b3DbvtNode* temp=leaves[begin]; + leaves[begin]=leaves[end]; + leaves[end]=temp; + ++begin; + } + + return begin; +} + +// +static b3DbvtVolume b3Bounds( b3DbvtNode** leaves, + int count) +{ +#if B3_DBVT_MERGE_IMPL==B3_DBVT_IMPL_SSE + B3_ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtVolume)]); + b3DbvtVolume& volume=*(b3DbvtVolume*)locals; + volume=leaves[0]->volume; +#else + b3DbvtVolume volume=leaves[0]->volume; +#endif + for(int i=1,ni=count;ivolume,volume); + } + return(volume); +} + +// +static void b3BottomUp( b3DynamicBvh* pdbvt, + b3DbvtNode** leaves, + int count) +{ + while(count>1) + { + b3Scalar minsize=B3_INFINITY; + int minidx[2]={-1,-1}; + for(int i=0;ivolume,leaves[j]->volume)); + if(szvolume,n[1]->volume,0); + p->childs[0] = n[0]; + p->childs[1] = n[1]; + n[0]->parent = p; + n[1]->parent = p; + leaves[minidx[0]] = p; + leaves[minidx[1]] = leaves[count-1]; + --count; + } +} + +// +static b3DbvtNode* b3TopDown(b3DynamicBvh* pdbvt, + b3DbvtNode** leaves, + int count, + int bu_treshold) +{ + static const b3Vector3 axis[]={b3MakeVector3(1,0,0), + b3MakeVector3(0,1,0), + b3MakeVector3(0,0,1)}; + b3Assert(bu_treshold>1); + if(count>1) + { + if(count>bu_treshold) + { + const b3DbvtVolume vol=b3Bounds(leaves,count); + const b3Vector3 org=vol.Center(); + int partition; + int bestaxis=-1; + int bestmidp=count; + int splitcount[3][2]={{0,0},{0,0},{0,0}}; + int i; + for( i=0;ivolume.Center()-org; + for(int j=0;j<3;++j) + { + ++splitcount[j][b3Dot(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)b3Fabs(b3Scalar(splitcount[i][0]-splitcount[i][1])); + if(midp=0) + { + partition=b3Split(leaves,count,org,axis[bestaxis]); + b3Assert(partition!=0 && partition!=count); + } + else + { + partition=count/2+1; + } + b3DbvtNode* node=b3CreateNode(pdbvt,0,vol,0); + node->childs[0]=b3TopDown(pdbvt,&leaves[0],partition,bu_treshold); + node->childs[1]=b3TopDown(pdbvt,&leaves[partition],count-partition,bu_treshold); + node->childs[0]->parent=node; + node->childs[1]->parent=node; + return(node); + } + else + { + b3BottomUp(pdbvt,leaves,count); + return(leaves[0]); + } + } + return(leaves[0]); +} + +// +static B3_DBVT_INLINE b3DbvtNode* b3Sort(b3DbvtNode* n,b3DbvtNode*& r) +{ + b3DbvtNode* p=n->parent; + b3Assert(n->isinternal()); + if(p>n) + { + const int i=b3IndexOf(n); + const int j=1-i; + b3DbvtNode* s=p->childs[j]; + b3DbvtNode* q=p->parent; + b3Assert(n==p->childs[i]); + if(q) q->childs[b3IndexOf(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; + b3Swap(p->volume,n->volume); + return(p); + } + return(n); +} + +#if 0 +static B3_DBVT_INLINE b3DbvtNode* walkup(b3DbvtNode* n,int count) +{ + while(n&&(count--)) n=n->parent; + return(n); +} +#endif + +// +// Api +// + +// +b3DynamicBvh::b3DynamicBvh() +{ + m_root = 0; + m_free = 0; + m_lkhd = -1; + m_leaves = 0; + m_opath = 0; +} + +// +b3DynamicBvh::~b3DynamicBvh() +{ + clear(); +} + +// +void b3DynamicBvh::clear() +{ + if(m_root) + b3RecurseDeleteNode(this,m_root); + b3AlignedFree(m_free); + m_free=0; + m_lkhd = -1; + m_stkStack.clear(); + m_opath = 0; + +} + +// +void b3DynamicBvh::optimizeBottomUp() +{ + if(m_root) + { + b3NodeArray leaves; + leaves.reserve(m_leaves); + b3FetchLeaves(this,m_root,leaves); + b3BottomUp(this,&leaves[0],leaves.size()); + m_root=leaves[0]; + } +} + +// +void b3DynamicBvh::optimizeTopDown(int bu_treshold) +{ + if(m_root) + { + b3NodeArray leaves; + leaves.reserve(m_leaves); + b3FetchLeaves(this,m_root,leaves); + m_root=b3TopDown(this,&leaves[0],leaves.size(),bu_treshold); + } +} + +// +void b3DynamicBvh::optimizeIncremental(int passes) +{ + if(passes<0) passes=m_leaves; + if(m_root&&(passes>0)) + { + do { + b3DbvtNode* node=m_root; + unsigned bit=0; + while(node->isinternal()) + { + node=b3Sort(node,m_root)->childs[(m_opath>>bit)&1]; + bit=(bit+1)&(sizeof(unsigned)*8-1); + } + update(node); + ++m_opath; + } while(--passes); + } +} + +// +b3DbvtNode* b3DynamicBvh::insert(const b3DbvtVolume& volume,void* data) +{ + b3DbvtNode* leaf=b3CreateNode(this,0,volume,data); + b3InsertLeaf(this,m_root,leaf); + ++m_leaves; + return(leaf); +} + +// +void b3DynamicBvh::update(b3DbvtNode* leaf,int lookahead) +{ + b3DbvtNode* root=b3RemoveLeaf(this,leaf); + if(root) + { + if(lookahead>=0) + { + for(int i=0;(iparent;++i) + { + root=root->parent; + } + } else root=m_root; + } + b3InsertLeaf(this,root,leaf); +} + +// +void b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume) +{ + b3DbvtNode* root=b3RemoveLeaf(this,leaf); + if(root) + { + if(m_lkhd>=0) + { + for(int i=0;(iparent;++i) + { + root=root->parent; + } + } else root=m_root; + } + leaf->volume=volume; + b3InsertLeaf(this,root,leaf); +} + +// +bool b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity,b3Scalar margin) +{ + if(leaf->volume.Contain(volume)) return(false); + volume.Expand(b3MakeVector3(margin,margin,margin)); + volume.SignedExpand(velocity); + update(leaf,volume); + return(true); +} + +// +bool b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity) +{ + if(leaf->volume.Contain(volume)) return(false); + volume.SignedExpand(velocity); + update(leaf,volume); + return(true); +} + +// +bool b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume,b3Scalar margin) +{ + if(leaf->volume.Contain(volume)) return(false); + volume.Expand(b3MakeVector3(margin,margin,margin)); + update(leaf,volume); + return(true); +} + +// +void b3DynamicBvh::remove(b3DbvtNode* leaf) +{ + b3RemoveLeaf(this,leaf); + b3DeleteNode(this,leaf); + --m_leaves; +} + +// +void b3DynamicBvh::write(IWriter* iwriter) const +{ + b3DbvtNodeEnumerator nodes; + nodes.nodes.reserve(m_leaves*2); + enumNodes(m_root,nodes); + iwriter->Prepare(m_root,nodes.nodes.size()); + for(int i=0;iparent) 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 b3DynamicBvh::clone(b3DynamicBvh& dest,IClone* iclone) const +{ + dest.clear(); + if(m_root!=0) + { + b3AlignedObjectArray stack; + stack.reserve(m_leaves); + stack.push_back(sStkCLN(m_root,0)); + do { + const int i=stack.size()-1; + const sStkCLN e=stack[i]; + b3DbvtNode* n=b3CreateNode(&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 b3DynamicBvh::maxdepth(const b3DbvtNode* node) +{ + int depth=0; + if(node) b3GetMaxDepth(node,1,depth); + return(depth); +} + +// +int b3DynamicBvh::countLeaves(const b3DbvtNode* node) +{ + if(node->isinternal()) + return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); + else + return(1); +} + +// +void b3DynamicBvh::extractLeaves(const b3DbvtNode* node,b3AlignedObjectArray& leaves) +{ + if(node->isinternal()) + { + extractLeaves(node->childs[0],leaves); + extractLeaves(node->childs[1],leaves); + } + else + { + leaves.push_back(node); + } +} + +// +#if B3_DBVT_ENABLE_BENCHMARK + +#include +#include + + +/* +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(b3DbvtVolume): 32 bytes +sizeof(b3DbvtNode): 44 bytes +[1] b3DbvtVolume intersections: 3499 ms (-1%) +[2] b3DbvtVolume merges: 1934 ms (0%) +[3] b3DynamicBvh::collideTT: 5485 ms (-21%) +[4] b3DynamicBvh::collideTT self: 2814 ms (-20%) +[5] b3DynamicBvh::collideTT xform: 7379 ms (-1%) +[6] b3DynamicBvh::collideTT xform,self: 7270 ms (-2%) +[7] b3DynamicBvh::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] b3DbvtVolume 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] b3DbvtVolume select: 3419 ms (0%) +*/ + +struct b3DbvtBenchmark +{ + struct NilPolicy : b3DynamicBvh::ICollide + { + NilPolicy() : m_pcount(0),m_depth(-B3_INFINITY),m_checksort(true) {} + void Process(const b3DbvtNode*,const b3DbvtNode*) { ++m_pcount; } + void Process(const b3DbvtNode*) { ++m_pcount; } + void Process(const b3DbvtNode*,b3Scalar 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; + b3Scalar m_depth; + bool m_checksort; + }; + struct P14 : b3DynamicBvh::ICollide + { + struct Node + { + const b3DbvtNode* leaf; + b3Scalar depth; + }; + void Process(const b3DbvtNode* leaf,b3Scalar depth) + { + Node n; + n.leaf = leaf; + n.depth = depth; + } + static int sortfnc(const Node& a,const Node& b) + { + if(a.depthb.depth) return(-1); + return(0); + } + b3AlignedObjectArray m_nodes; + }; + struct P15 : b3DynamicBvh::ICollide + { + struct Node + { + const b3DbvtNode* leaf; + b3Scalar depth; + }; + void Process(const b3DbvtNode* 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.depthb.depth) return(-1); + return(0); + } + b3AlignedObjectArray m_nodes; + b3Vector3 m_axis; + }; + static b3Scalar RandUnit() + { + return(rand()/(b3Scalar)RAND_MAX); + } + static b3Vector3 RandVector3() + { + return(b3Vector3(RandUnit(),RandUnit(),RandUnit())); + } + static b3Vector3 RandVector3(b3Scalar cs) + { + return(RandVector3()*cs-b3Vector3(cs,cs,cs)/2); + } + static b3DbvtVolume RandVolume(b3Scalar cs,b3Scalar eb,b3Scalar es) + { + return(b3DbvtVolume::FromCE(RandVector3(cs),b3Vector3(eb,eb,eb)+RandVector3()*es)); + } + static b3Transform RandTransform(b3Scalar cs) + { + b3Transform t; + t.setOrigin(RandVector3(cs)); + t.setRotation(b3Quaternion(RandUnit()*B3_PI*2,RandUnit()*B3_PI*2,RandUnit()*B3_PI*2).normalized()); + return(t); + } + static void RandTree(b3Scalar cs,b3Scalar eb,b3Scalar es,int leaves,b3DynamicBvh& dbvt) + { + dbvt.clear(); + for(int i=0;i volumes; + b3AlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i volumes; + b3AlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i transforms; + b3DbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark5_Iterations); + for(int i=0;i transforms; + b3DbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark6_Iterations); + for(int i=0;i rayorg; + b3AlignedObjectArray raydir; + b3DbvtBenchmark::NilPolicy policy; + rayorg.resize(cfgBenchmark7_Iterations); + raydir.resize(cfgBenchmark7_Iterations); + for(int i=0;i leaves; + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + dbvt.extractLeaves(dbvt.m_root,leaves); + printf("[9] updates (teleport): "); + wallclock.reset(); + for(int i=0;i(leaves[rand()%cfgLeaves]), + b3DbvtBenchmark::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); + b3DynamicBvh dbvt; + b3AlignedObjectArray leaves; + b3AlignedObjectArray vectors; + vectors.resize(cfgBenchmark10_Iterations); + for(int i=0;i(leaves[rand()%cfgLeaves]); + b3DbvtVolume v=b3DbvtVolume::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); + b3DynamicBvh dbvt; + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[11] optimize (incremental): "); + wallclock.reset(); + for(int i=0;i volumes; + b3AlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i vectors; + b3DbvtBenchmark::NilPolicy policy; + vectors.resize(cfgBenchmark13_Iterations); + for(int i=0;i vectors; + b3DbvtBenchmark::P14 policy; + vectors.resize(cfgBenchmark14_Iterations); + for(int i=0;i vectors; + b3DbvtBenchmark::P15 policy; + vectors.resize(cfgBenchmark15_Iterations); + for(int i=0;i batch; + b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + batch.reserve(cfgBenchmark16_BatchCount); + printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount); + wallclock.reset(); + for(int i=0;i volumes; + b3AlignedObjectArray results; + b3AlignedObjectArray indices; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + indices.resize(cfgLeaves); + for(int i=0;i= 1400) +#define B3_DBVT_USE_TEMPLATE 1 +#else +#define B3_DBVT_USE_TEMPLATE 0 +#endif +#else +#define B3_DBVT_USE_TEMPLATE 0 +#endif + +// Use only intrinsics instead of inline asm +#define B3_DBVT_USE_INTRINSIC_SSE 1 + +// Using memmov for collideOCL +#define B3_DBVT_USE_MEMMOVE 1 + +// Enable benchmarking code +#define B3_DBVT_ENABLE_BENCHMARK 0 + +// Inlining +#define B3_DBVT_INLINE B3_FORCE_INLINE + +// Specific methods implementation + +//SSE gives errors on a MSVC 7.1 +#if defined (B3_USE_SSE) //&& defined (_WIN32) +#define B3_DBVT_SELECT_IMPL B3_DBVT_IMPL_SSE +#define B3_DBVT_MERGE_IMPL B3_DBVT_IMPL_SSE +#define B3_DBVT_INT0_IMPL B3_DBVT_IMPL_SSE +#else +#define B3_DBVT_SELECT_IMPL B3_DBVT_IMPL_GENERIC +#define B3_DBVT_MERGE_IMPL B3_DBVT_IMPL_GENERIC +#define B3_DBVT_INT0_IMPL B3_DBVT_IMPL_GENERIC +#endif + +#if (B3_DBVT_SELECT_IMPL==B3_DBVT_IMPL_SSE)|| \ + (B3_DBVT_MERGE_IMPL==B3_DBVT_IMPL_SSE)|| \ + (B3_DBVT_INT0_IMPL==B3_DBVT_IMPL_SSE) +#include +#endif + +// +// Auto config and checks +// + +#if B3_DBVT_USE_TEMPLATE +#define B3_DBVT_VIRTUAL +#define B3_DBVT_VIRTUAL_DTOR(a) +#define B3_DBVT_PREFIX template +#define B3_DBVT_IPOLICY T& policy +#define B3_DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker; +#else +#define B3_DBVT_VIRTUAL_DTOR(a) virtual ~a() {} +#define B3_DBVT_VIRTUAL virtual +#define B3_DBVT_PREFIX +#define B3_DBVT_IPOLICY ICollide& policy +#define B3_DBVT_CHECKTYPE +#endif + +#if B3_DBVT_USE_MEMMOVE +#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) +#include +#endif +#include +#endif + +#ifndef B3_DBVT_USE_TEMPLATE +#error "B3_DBVT_USE_TEMPLATE undefined" +#endif + +#ifndef B3_DBVT_USE_MEMMOVE +#error "B3_DBVT_USE_MEMMOVE undefined" +#endif + +#ifndef B3_DBVT_ENABLE_BENCHMARK +#error "B3_DBVT_ENABLE_BENCHMARK undefined" +#endif + +#ifndef B3_DBVT_SELECT_IMPL +#error "B3_DBVT_SELECT_IMPL undefined" +#endif + +#ifndef B3_DBVT_MERGE_IMPL +#error "B3_DBVT_MERGE_IMPL undefined" +#endif + +#ifndef B3_DBVT_INT0_IMPL +#error "B3_DBVT_INT0_IMPL undefined" +#endif + +// +// Defaults volumes +// + +/* b3DbvtAabbMm */ +struct b3DbvtAabbMm +{ + B3_DBVT_INLINE b3Vector3 Center() const { return((mi+mx)/2); } + B3_DBVT_INLINE b3Vector3 Lengths() const { return(mx-mi); } + B3_DBVT_INLINE b3Vector3 Extents() const { return((mx-mi)/2); } + B3_DBVT_INLINE const b3Vector3& Mins() const { return(mi); } + B3_DBVT_INLINE const b3Vector3& Maxs() const { return(mx); } + static inline b3DbvtAabbMm FromCE(const b3Vector3& c,const b3Vector3& e); + static inline b3DbvtAabbMm FromCR(const b3Vector3& c,b3Scalar r); + static inline b3DbvtAabbMm FromMM(const b3Vector3& mi,const b3Vector3& mx); + static inline b3DbvtAabbMm FromPoints(const b3Vector3* pts,int n); + static inline b3DbvtAabbMm FromPoints(const b3Vector3** ppts,int n); + B3_DBVT_INLINE void Expand(const b3Vector3& e); + B3_DBVT_INLINE void SignedExpand(const b3Vector3& e); + B3_DBVT_INLINE bool Contain(const b3DbvtAabbMm& a) const; + B3_DBVT_INLINE int Classify(const b3Vector3& n,b3Scalar o,int s) const; + B3_DBVT_INLINE b3Scalar ProjectMinimum(const b3Vector3& v,unsigned signs) const; + B3_DBVT_INLINE friend bool b3Intersect( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b); + + B3_DBVT_INLINE friend bool b3Intersect( const b3DbvtAabbMm& a, + const b3Vector3& b); + + B3_DBVT_INLINE friend b3Scalar b3Proximity( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b); + B3_DBVT_INLINE friend int b3Select( const b3DbvtAabbMm& o, + const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b); + B3_DBVT_INLINE friend void b3Merge( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b, + b3DbvtAabbMm& r); + B3_DBVT_INLINE friend bool b3NotEqual( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b); + + B3_DBVT_INLINE b3Vector3& tMins() { return(mi); } + B3_DBVT_INLINE b3Vector3& tMaxs() { return(mx); } + +private: + B3_DBVT_INLINE void AddSpan(const b3Vector3& d,b3Scalar& smi,b3Scalar& smx) const; +private: + b3Vector3 mi,mx; +}; + +// Types +typedef b3DbvtAabbMm b3DbvtVolume; + +/* b3DbvtNode */ +struct b3DbvtNode +{ + b3DbvtVolume volume; + b3DbvtNode* parent; + B3_DBVT_INLINE bool isleaf() const { return(childs[1]==0); } + B3_DBVT_INLINE bool isinternal() const { return(!isleaf()); } + union + { + b3DbvtNode* childs[2]; + void* data; + int dataAsInt; + }; +}; + +///The b3DynamicBvh class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). +///This b3DynamicBvh is used for soft body collision detection and for the b3DynamicBvhBroadphase. It has a fast insert, remove and update of nodes. +///Unlike the b3QuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. +struct b3DynamicBvh +{ + /* Stack element */ + struct sStkNN + { + const b3DbvtNode* a; + const b3DbvtNode* b; + sStkNN() {} + sStkNN(const b3DbvtNode* na,const b3DbvtNode* nb) : a(na),b(nb) {} + }; + struct sStkNP + { + const b3DbvtNode* node; + int mask; + sStkNP(const b3DbvtNode* n,unsigned m) : node(n),mask(m) {} + }; + struct sStkNPS + { + const b3DbvtNode* node; + int mask; + b3Scalar value; + sStkNPS() {} + sStkNPS(const b3DbvtNode* n,unsigned m,b3Scalar v) : node(n),mask(m),value(v) {} + }; + struct sStkCLN + { + const b3DbvtNode* node; + b3DbvtNode* parent; + sStkCLN(const b3DbvtNode* n,b3DbvtNode* p) : node(n),parent(p) {} + }; + // Policies/Interfaces + + /* ICollide */ + struct ICollide + { + B3_DBVT_VIRTUAL_DTOR(ICollide) + B3_DBVT_VIRTUAL void Process(const b3DbvtNode*,const b3DbvtNode*) {} + B3_DBVT_VIRTUAL void Process(const b3DbvtNode*) {} + B3_DBVT_VIRTUAL void Process(const b3DbvtNode* n,b3Scalar) { Process(n); } + B3_DBVT_VIRTUAL bool Descent(const b3DbvtNode*) { return(true); } + B3_DBVT_VIRTUAL bool AllLeaves(const b3DbvtNode*) { return(true); } + }; + /* IWriter */ + struct IWriter + { + virtual ~IWriter() {} + virtual void Prepare(const b3DbvtNode* root,int numnodes)=0; + virtual void WriteNode(const b3DbvtNode*,int index,int parent,int child0,int child1)=0; + virtual void WriteLeaf(const b3DbvtNode*,int index,int parent)=0; + }; + /* IClone */ + struct IClone + { + virtual ~IClone() {} + virtual void CloneLeaf(b3DbvtNode*) {} + }; + + // Constants + enum { + B3_SIMPLE_STACKSIZE = 64, + B3_DOUBLE_STACKSIZE = B3_SIMPLE_STACKSIZE*2 + }; + + // Fields + b3DbvtNode* m_root; + b3DbvtNode* m_free; + int m_lkhd; + int m_leaves; + unsigned m_opath; + + + b3AlignedObjectArray m_stkStack; + mutable b3AlignedObjectArray m_rayTestStack; + + + // Methods + b3DynamicBvh(); + ~b3DynamicBvh(); + void clear(); + bool empty() const { return(0==m_root); } + void optimizeBottomUp(); + void optimizeTopDown(int bu_treshold=128); + void optimizeIncremental(int passes); + b3DbvtNode* insert(const b3DbvtVolume& box,void* data); + void update(b3DbvtNode* leaf,int lookahead=-1); + void update(b3DbvtNode* leaf,b3DbvtVolume& volume); + bool update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity,b3Scalar margin); + bool update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity); + bool update(b3DbvtNode* leaf,b3DbvtVolume& volume,b3Scalar margin); + void remove(b3DbvtNode* leaf); + void write(IWriter* iwriter) const; + void clone(b3DynamicBvh& dest,IClone* iclone=0) const; + static int maxdepth(const b3DbvtNode* node); + static int countLeaves(const b3DbvtNode* node); + static void extractLeaves(const b3DbvtNode* node,b3AlignedObjectArray& leaves); +#if B3_DBVT_ENABLE_BENCHMARK + static void benchmark(); +#else + static void benchmark(){} +#endif + // B3_DBVT_IPOLICY must support ICollide policy/interface + B3_DBVT_PREFIX + static void enumNodes( const b3DbvtNode* root, + B3_DBVT_IPOLICY); + B3_DBVT_PREFIX + static void enumLeaves( const b3DbvtNode* root, + B3_DBVT_IPOLICY); + B3_DBVT_PREFIX + void collideTT( const b3DbvtNode* root0, + const b3DbvtNode* root1, + B3_DBVT_IPOLICY); + + B3_DBVT_PREFIX + void collideTTpersistentStack( const b3DbvtNode* root0, + const b3DbvtNode* root1, + B3_DBVT_IPOLICY); +#if 0 + B3_DBVT_PREFIX + void collideTT( const b3DbvtNode* root0, + const b3DbvtNode* root1, + const b3Transform& xform, + B3_DBVT_IPOLICY); + B3_DBVT_PREFIX + void collideTT( const b3DbvtNode* root0, + const b3Transform& xform0, + const b3DbvtNode* root1, + const b3Transform& xform1, + B3_DBVT_IPOLICY); +#endif + + B3_DBVT_PREFIX + void collideTV( const b3DbvtNode* root, + const b3DbvtVolume& volume, + B3_DBVT_IPOLICY) const; + ///rayTest is a re-entrant ray test, and can be called in parallel as long as the b3AlignedAlloc is thread-safe (uses locking etc) + ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time + B3_DBVT_PREFIX + static void rayTest( const b3DbvtNode* root, + const b3Vector3& rayFrom, + const b3Vector3& rayTo, + B3_DBVT_IPOLICY); + ///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections + ///rayTestInternal is used by b3DynamicBvhBroadphase to accelerate world ray casts + B3_DBVT_PREFIX + void rayTestInternal( const b3DbvtNode* root, + const b3Vector3& rayFrom, + const b3Vector3& rayTo, + const b3Vector3& rayDirectionInverse, + unsigned int signs[3], + b3Scalar lambda_max, + const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + B3_DBVT_IPOLICY) const; + + B3_DBVT_PREFIX + static void collideKDOP(const b3DbvtNode* root, + const b3Vector3* normals, + const b3Scalar* offsets, + int count, + B3_DBVT_IPOLICY); + B3_DBVT_PREFIX + static void collideOCL( const b3DbvtNode* root, + const b3Vector3* normals, + const b3Scalar* offsets, + const b3Vector3& sortaxis, + int count, + B3_DBVT_IPOLICY, + bool fullsort=true); + B3_DBVT_PREFIX + static void collideTU( const b3DbvtNode* root, + B3_DBVT_IPOLICY); + // Helpers + static B3_DBVT_INLINE int nearest(const int* i,const b3DynamicBvh::sStkNPS* a,b3Scalar v,int l,int h) + { + int m=0; + while(l>1; + if(a[i[m]].value>=v) l=m+1; else h=m; + } + return(h); + } + static B3_DBVT_INLINE int allocate( b3AlignedObjectArray& ifree, + b3AlignedObjectArray& stock, + const sStkNPS& value) + { + int i; + if(ifree.size()>0) + { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; } + else + { i=stock.size();stock.push_back(value); } + return(i); + } + // +private: + b3DynamicBvh(const b3DynamicBvh&) {} +}; + +// +// Inline's +// + +// +inline b3DbvtAabbMm b3DbvtAabbMm::FromCE(const b3Vector3& c,const b3Vector3& e) +{ + b3DbvtAabbMm box; + box.mi=c-e;box.mx=c+e; + return(box); +} + +// +inline b3DbvtAabbMm b3DbvtAabbMm::FromCR(const b3Vector3& c,b3Scalar r) +{ + return(FromCE(c,b3MakeVector3(r,r,r))); +} + +// +inline b3DbvtAabbMm b3DbvtAabbMm::FromMM(const b3Vector3& mi,const b3Vector3& mx) +{ + b3DbvtAabbMm box; + box.mi=mi;box.mx=mx; + return(box); +} + +// +inline b3DbvtAabbMm b3DbvtAabbMm::FromPoints(const b3Vector3* pts,int n) +{ + b3DbvtAabbMm box; + box.mi=box.mx=pts[0]; + for(int i=1;i0) mx.setX(mx.x+e[0]); else mi.setX(mi.x+e[0]); + if(e.y>0) mx.setY(mx.y+e[1]); else mi.setY(mi.y+e[1]); + if(e.z>0) mx.setZ(mx.z+e[2]); else mi.setZ(mi.z+e[2]); +} + +// +B3_DBVT_INLINE bool b3DbvtAabbMm::Contain(const b3DbvtAabbMm& a) const +{ + return( (mi.x<=a.mi.x)&& + (mi.y<=a.mi.y)&& + (mi.z<=a.mi.z)&& + (mx.x>=a.mx.x)&& + (mx.y>=a.mx.y)&& + (mx.z>=a.mx.z)); +} + +// +B3_DBVT_INLINE int b3DbvtAabbMm::Classify(const b3Vector3& n,b3Scalar o,int s) const +{ + b3Vector3 pi,px; + switch(s) + { + case (0+0+0): px=b3MakeVector3(mi.x,mi.y,mi.z); + pi=b3MakeVector3(mx.x,mx.y,mx.z);break; + case (1+0+0): px=b3MakeVector3(mx.x,mi.y,mi.z); + pi=b3MakeVector3(mi.x,mx.y,mx.z);break; + case (0+2+0): px=b3MakeVector3(mi.x,mx.y,mi.z); + pi=b3MakeVector3(mx.x,mi.y,mx.z);break; + case (1+2+0): px=b3MakeVector3(mx.x,mx.y,mi.z); + pi=b3MakeVector3(mi.x,mi.y,mx.z);break; + case (0+0+4): px=b3MakeVector3(mi.x,mi.y,mx.z); + pi=b3MakeVector3(mx.x,mx.y,mi.z);break; + case (1+0+4): px=b3MakeVector3(mx.x,mi.y,mx.z); + pi=b3MakeVector3(mi.x,mx.y,mi.z);break; + case (0+2+4): px=b3MakeVector3(mi.x,mx.y,mx.z); + pi=b3MakeVector3(mx.x,mi.y,mi.z);break; + case (1+2+4): px=b3MakeVector3(mx.x,mx.y,mx.z); + pi=b3MakeVector3(mi.x,mi.y,mi.z);break; + } + if((b3Dot(n,px)+o)<0) return(-1); + if((b3Dot(n,pi)+o)>=0) return(+1); + return(0); +} + +// +B3_DBVT_INLINE b3Scalar b3DbvtAabbMm::ProjectMinimum(const b3Vector3& v,unsigned signs) const +{ + const b3Vector3* b[]={&mx,&mi}; + const b3Vector3 p = b3MakeVector3( b[(signs>>0)&1]->x, + b[(signs>>1)&1]->y, + b[(signs>>2)&1]->z); + return(b3Dot(p,v)); +} + +// +B3_DBVT_INLINE void b3DbvtAabbMm::AddSpan(const b3Vector3& d,b3Scalar& smi,b3Scalar& smx) const +{ + for(int i=0;i<3;++i) + { + if(d[i]<0) + { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; } + else + { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; } + } +} + +// +B3_DBVT_INLINE bool b3Intersect( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b) +{ +#if B3_DBVT_INT0_IMPL == B3_DBVT_IMPL_SSE + const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), + _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); +#if defined (_WIN32) + const __int32* pu((const __int32*)&rt); +#else + const int* pu((const int*)&rt); +#endif + return((pu[0]|pu[1]|pu[2])==0); +#else + return( (a.mi.x<=b.mx.x)&& + (a.mx.x>=b.mi.x)&& + (a.mi.y<=b.mx.y)&& + (a.mx.y>=b.mi.y)&& + (a.mi.z<=b.mx.z)&& + (a.mx.z>=b.mi.z)); +#endif +} + + + +// +B3_DBVT_INLINE bool b3Intersect( const b3DbvtAabbMm& a, + const b3Vector3& b) +{ + return( (b.x>=a.mi.x)&& + (b.y>=a.mi.y)&& + (b.z>=a.mi.z)&& + (b.x<=a.mx.x)&& + (b.y<=a.mx.y)&& + (b.z<=a.mx.z)); +} + + + + + +////////////////////////////////////// + + +// +B3_DBVT_INLINE b3Scalar b3Proximity( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b) +{ + const b3Vector3 d=(a.mi+a.mx)-(b.mi+b.mx); + return(b3Fabs(d.x)+b3Fabs(d.y)+b3Fabs(d.z)); +} + + + +// +B3_DBVT_INLINE int b3Select( const b3DbvtAabbMm& o, + const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b) +{ +#if B3_DBVT_SELECT_IMPL == B3_DBVT_IMPL_SSE + +#if defined (_WIN32) + static B3_ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; +#else + static B3_ATTRIBUTE_ALIGNED16(const unsigned int) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x00000000 /*0x7fffffff*/}; +#endif + ///@todo: the intrinsic version is 11% slower +#if B3_DBVT_USE_INTRINSIC_SSE + + union b3SSEUnion ///NOTE: if we use more intrinsics, move b3SSEUnion into the LinearMath directory + { + __m128 ssereg; + float floats[4]; + int ints[4]; + }; + + __m128 omi(_mm_load_ps(o.mi)); + omi=_mm_add_ps(omi,_mm_load_ps(o.mx)); + __m128 ami(_mm_load_ps(a.mi)); + ami=_mm_add_ps(ami,_mm_load_ps(a.mx)); + ami=_mm_sub_ps(ami,omi); + ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask)); + __m128 bmi(_mm_load_ps(b.mi)); + bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx)); + bmi=_mm_sub_ps(bmi,omi); + bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask)); + __m128 t0(_mm_movehl_ps(ami,ami)); + ami=_mm_add_ps(ami,t0); + ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1)); + __m128 t1(_mm_movehl_ps(bmi,bmi)); + bmi=_mm_add_ps(bmi,t1); + bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1)); + + b3SSEUnion tmp; + tmp.ssereg = _mm_cmple_ss(bmi,ami); + return tmp.ints[0]&1; + +#else + B3_ATTRIBUTE_ALIGNED16(__int32 r[1]); + __asm + { + mov eax,o + mov ecx,a + mov edx,b + movaps xmm0,[eax] + movaps xmm5,mask + addps xmm0,[eax+16] + movaps xmm1,[ecx] + movaps xmm2,[edx] + addps xmm1,[ecx+16] + addps xmm2,[edx+16] + subps xmm1,xmm0 + subps xmm2,xmm0 + andps xmm1,xmm5 + andps xmm2,xmm5 + movhlps xmm3,xmm1 + movhlps xmm4,xmm2 + addps xmm1,xmm3 + addps xmm2,xmm4 + pshufd xmm3,xmm1,1 + pshufd xmm4,xmm2,1 + addss xmm1,xmm3 + addss xmm2,xmm4 + cmpless xmm2,xmm1 + movss r,xmm2 + } + return(r[0]&1); +#endif +#else + return(b3Proximity(o,a)b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; + } +#endif +} + +// +B3_DBVT_INLINE bool b3NotEqual( const b3DbvtAabbMm& a, + const b3DbvtAabbMm& b) +{ + return( (a.mi.x!=b.mi.x)|| + (a.mi.y!=b.mi.y)|| + (a.mi.z!=b.mi.z)|| + (a.mx.x!=b.mx.x)|| + (a.mx.y!=b.mx.y)|| + (a.mx.z!=b.mx.z)); +} + +// +// Inline's +// + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::enumNodes( const b3DbvtNode* root, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + policy.Process(root); + if(root->isinternal()) + { + enumNodes(root->childs[0],policy); + enumNodes(root->childs[1],policy); + } +} + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::enumLeaves( const b3DbvtNode* root, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root->isinternal()) + { + enumLeaves(root->childs[0],policy); + enumLeaves(root->childs[1],policy); + } + else + { + policy.Process(root); + } +} + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideTT( const b3DbvtNode* root0, + const b3DbvtNode* root1, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=B3_DOUBLE_STACKSIZE-4; + b3AlignedObjectArray stkStack; + stkStack.resize(B3_DOUBLE_STACKSIZE); + stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=stkStack[--depth]; + if(depth>treshold) + { + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + if(p.a==p.b) + { + if(p.a->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + } + } + else if(b3Intersect(p.a->volume,p.b->volume)) + { + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} + + + +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideTTpersistentStack( const b3DbvtNode* root0, + const b3DbvtNode* root1, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=B3_DOUBLE_STACKSIZE-4; + + m_stkStack.resize(B3_DOUBLE_STACKSIZE); + m_stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=m_stkStack[--depth]; + if(depth>treshold) + { + m_stkStack.resize(m_stkStack.size()*2); + treshold=m_stkStack.size()-4; + } + if(p.a==p.b) + { + if(p.a->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + } + } + else if(b3Intersect(p.a->volume,p.b->volume)) + { + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} + +#if 0 +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideTT( const b3DbvtNode* root0, + const b3DbvtNode* root1, + const b3Transform& xform, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=B3_DOUBLE_STACKSIZE-4; + b3AlignedObjectArray stkStack; + stkStack.resize(B3_DOUBLE_STACKSIZE); + stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=stkStack[--depth]; + if(b3Intersect(p.a->volume,p.b->volume,xform)) + { + if(depth>treshold) + { + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideTT( const b3DbvtNode* root0, + const b3Transform& xform0, + const b3DbvtNode* root1, + const b3Transform& xform1, + B3_DBVT_IPOLICY) +{ + const b3Transform xform=xform0.inverse()*xform1; + collideTT(root0,root1,xform,policy); +} +#endif + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideTV( const b3DbvtNode* root, + const b3DbvtVolume& vol, + B3_DBVT_IPOLICY) const +{ + B3_DBVT_CHECKTYPE + if(root) + { + B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) volume(vol); + b3AlignedObjectArray stack; + stack.resize(0); + stack.reserve(B3_SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const b3DbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(b3Intersect(n->volume,volume)) + { + if(n->isinternal()) + { + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); + } + else + { + policy.Process(n); + } + } + } while(stack.size()>0); + } +} + +B3_DBVT_PREFIX +inline void b3DynamicBvh::rayTestInternal( const b3DbvtNode* root, + const b3Vector3& rayFrom, + const b3Vector3& rayTo, + const b3Vector3& rayDirectionInverse, + unsigned int signs[3], + b3Scalar lambda_max, + const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + B3_DBVT_IPOLICY) const +{ + (void) rayTo; + B3_DBVT_CHECKTYPE + if(root) + { + int depth=1; + int treshold=B3_DOUBLE_STACKSIZE-2; + b3AlignedObjectArray& stack = m_rayTestStack; + stack.resize(B3_DOUBLE_STACKSIZE); + stack[0]=root; + b3Vector3 bounds[2]; + do + { + const b3DbvtNode* node=stack[--depth]; + bounds[0] = node->volume.Mins()-aabbMax; + bounds[1] = node->volume.Maxs()-aabbMin; + b3Scalar tmin=1.f,lambda_min=0.f; + unsigned int result1=false; + result1 = b3RayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + if(result1) + { + if(node->isinternal()) + { + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-2; + } + stack[depth++]=node->childs[0]; + stack[depth++]=node->childs[1]; + } + else + { + policy.Process(node); + } + } + } while(depth); + } +} + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::rayTest( const b3DbvtNode* root, + const b3Vector3& rayFrom, + const b3Vector3& rayTo, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root) + { + b3Vector3 rayDir = (rayTo-rayFrom); + rayDir.normalize (); + + ///what about division by zero? --> just set rayDirection[i] to INF/B3_LARGE_FLOAT + b3Vector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[2]; + unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; + + b3Scalar lambda_max = rayDir.dot(rayTo-rayFrom); +#ifdef COMPARE_BTRAY_AABB2 + b3Vector3 resultNormal; +#endif//COMPARE_BTRAY_AABB2 + + b3AlignedObjectArray stack; + + int depth=1; + int treshold=B3_DOUBLE_STACKSIZE-2; + + stack.resize(B3_DOUBLE_STACKSIZE); + stack[0]=root; + b3Vector3 bounds[2]; + do { + const b3DbvtNode* node=stack[--depth]; + + bounds[0] = node->volume.Mins(); + bounds[1] = node->volume.Maxs(); + + b3Scalar tmin=1.f,lambda_min=0.f; + unsigned int result1 = b3RayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + +#ifdef COMPARE_BTRAY_AABB2 + b3Scalar param=1.f; + bool result2 = b3RayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); + b3Assert(result1 == result2); +#endif //TEST_BTRAY_AABB2 + + if(result1) + { + if(node->isinternal()) + { + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-2; + } + stack[depth++]=node->childs[0]; + stack[depth++]=node->childs[1]; + } + else + { + policy.Process(node); + } + } + } while(depth); + + } +} + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideKDOP(const b3DbvtNode* root, + const b3Vector3* normals, + const b3Scalar* offsets, + int count, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root) + { + const int inside=(1< stack; + int signs[sizeof(unsigned)*8]; + b3Assert(count=0)?1:0)+ + ((normals[i].y>=0)?2:0)+ + ((normals[i].z>=0)?4:0); + } + stack.reserve(B3_SIMPLE_STACKSIZE); + stack.push_back(sStkNP(root,0)); + do { + sStkNP se=stack[stack.size()-1]; + bool out=false; + stack.pop_back(); + for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); + switch(side) + { + case -1: out=true;break; + case +1: se.mask|=j;break; + } + } + } + if(!out) + { + if((se.mask!=inside)&&(se.node->isinternal())) + { + stack.push_back(sStkNP(se.node->childs[0],se.mask)); + stack.push_back(sStkNP(se.node->childs[1],se.mask)); + } + else + { + if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); + } + } + } while(stack.size()); + } +} + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideOCL( const b3DbvtNode* root, + const b3Vector3* normals, + const b3Scalar* offsets, + const b3Vector3& sortaxis, + int count, + B3_DBVT_IPOLICY, + bool fsort) +{ + B3_DBVT_CHECKTYPE + if(root) + { + const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ + (sortaxis[1]>=0?2:0)+ + (sortaxis[2]>=0?4:0); + const int inside=(1< stock; + b3AlignedObjectArray ifree; + b3AlignedObjectArray stack; + int signs[sizeof(unsigned)*8]; + b3Assert(count=0)?1:0)+ + ((normals[i].y>=0)?2:0)+ + ((normals[i].z>=0)?4:0); + } + stock.reserve(B3_SIMPLE_STACKSIZE); + stack.reserve(B3_SIMPLE_STACKSIZE); + ifree.reserve(B3_SIMPLE_STACKSIZE); + stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)))); + do { + const int id=stack[stack.size()-1]; + sStkNPS se=stock[id]; + stack.pop_back();ifree.push_back(id); + if(se.mask!=inside) + { + bool out=false; + for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); + switch(side) + { + case -1: out=true;break; + case +1: se.mask|=j;break; + } + } + } + if(out) continue; + } + if(policy.Descent(se.node)) + { + if(se.node->isinternal()) + { + const b3DbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]}; + sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)), + sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))}; + const int q=nes[0].value0)) + { + /* Insert 0 */ + j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); + stack.push_back(0); +#if B3_DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); +#else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; +#endif + stack[j]=allocate(ifree,stock,nes[q]); + /* Insert 1 */ + j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); + stack.push_back(0); +#if B3_DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); +#else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; +#endif + stack[j]=allocate(ifree,stock,nes[1-q]); + } + else + { + stack.push_back(allocate(ifree,stock,nes[q])); + stack.push_back(allocate(ifree,stock,nes[1-q])); + } + } + else + { + policy.Process(se.node,se.value); + } + } + } while(stack.size()); + } +} + +// +B3_DBVT_PREFIX +inline void b3DynamicBvh::collideTU( const b3DbvtNode* root, + B3_DBVT_IPOLICY) +{ + B3_DBVT_CHECKTYPE + if(root) + { + b3AlignedObjectArray stack; + stack.reserve(B3_SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const b3DbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(policy.Descent(n)) + { + if(n->isinternal()) + { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } + else + { policy.Process(n); } + } + } while(stack.size()>0); + } +} + +// +// PP Cleanup +// + +#undef B3_DBVT_USE_MEMMOVE +#undef B3_DBVT_USE_TEMPLATE +#undef B3_DBVT_VIRTUAL_DTOR +#undef B3_DBVT_VIRTUAL +#undef B3_DBVT_PREFIX +#undef B3_DBVT_IPOLICY +#undef B3_DBVT_CHECKTYPE +#undef B3_DBVT_IMPL_GENERIC +#undef B3_DBVT_IMPL_SSE +#undef B3_DBVT_USE_INTRINSIC_SSE +#undef B3_DBVT_SELECT_IMPL +#undef B3_DBVT_MERGE_IMPL +#undef B3_DBVT_INT0_IMPL + +#endif diff --git a/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp new file mode 100644 index 000000000000..bc150955b838 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp @@ -0,0 +1,804 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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. +*/ + +///b3DynamicBvhBroadphase implementation by Nathanael Presson + +#include "b3DynamicBvhBroadphase.h" +#include "b3OverlappingPair.h" + +// +// Profiling +// + +#if B3_DBVT_BP_PROFILE||B3_DBVT_BP_ENABLE_BENCHMARK +#include +#endif + +#if B3_DBVT_BP_PROFILE +struct b3ProfileScope +{ + __forceinline b3ProfileScope(b3Clock& clock,unsigned long& value) : + m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) + { + } + __forceinline ~b3ProfileScope() + { + (*m_value)+=m_clock->getTimeMicroseconds()-m_base; + } + b3Clock* m_clock; + unsigned long* m_value; + unsigned long m_base; +}; +#define b3SPC(_value_) b3ProfileScope spc_scope(m_clock,_value_) +#else +#define b3SPC(_value_) +#endif + +// +// Helpers +// + +// +template +static inline void b3ListAppend(T* item,T*& list) +{ + item->links[0]=0; + item->links[1]=list; + if(list) list->links[0]=item; + list=item; +} + +// +template +static inline void b3ListRemove(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 +static inline int b3ListCount(T* root) +{ + int n=0; + while(root) { ++n;root=root->links[1]; } + return(n); +} + +// +template +static inline void b3Clear(T& value) +{ + static const struct ZeroDummy : T {} zerodummy; + value=zerodummy; +} + +// +// Colliders +// + +/* Tree collider */ +struct b3DbvtTreeCollider : b3DynamicBvh::ICollide +{ + b3DynamicBvhBroadphase* pbp; + b3DbvtProxy* proxy; + b3DbvtTreeCollider(b3DynamicBvhBroadphase* p) : pbp(p) {} + void Process(const b3DbvtNode* na,const b3DbvtNode* nb) + { + if(na!=nb) + { + b3DbvtProxy* pa=(b3DbvtProxy*)na->data; + b3DbvtProxy* pb=(b3DbvtProxy*)nb->data; +#if B3_DBVT_BP_SORTPAIRS + if(pa->m_uniqueId>pb->m_uniqueId) + b3Swap(pa,pb); +#endif + pbp->m_paircache->addOverlappingPair(pa->getUid(),pb->getUid()); + ++pbp->m_newpairs; + } + } + void Process(const b3DbvtNode* n) + { + Process(n,proxy->leaf); + } +}; + +// +// b3DynamicBvhBroadphase +// + +// +b3DynamicBvhBroadphase::b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* 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(b3AlignedAlloc(sizeof(b3HashedOverlappingPairCache),16)) b3HashedOverlappingPairCache(); + + m_pid = 0; + m_cid = 0; + for(int i=0;i<=STAGECOUNT;++i) + { + m_stageRoots[i]=0; + } +#if B3_DBVT_BP_PROFILE + b3Clear(m_profiling); +#endif + m_proxies.resize(proxyCapacity); +} + +// +b3DynamicBvhBroadphase::~b3DynamicBvhBroadphase() +{ + if(m_releasepaircache) + { + m_paircache->~b3OverlappingPairCache(); + b3AlignedFree(m_paircache); + } +} + +// +b3BroadphaseProxy* b3DynamicBvhBroadphase::createProxy( const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + int objectId, + void* userPtr, + int collisionFilterGroup, + int collisionFilterMask) +{ + b3DbvtProxy* mem = &m_proxies[objectId]; + b3DbvtProxy* proxy=new(mem) b3DbvtProxy( aabbMin,aabbMax,userPtr, + collisionFilterGroup, + collisionFilterMask); + + b3DbvtAabbMm aabb = b3DbvtVolume::FromMM(aabbMin,aabbMax); + + //bproxy->aabb = b3DbvtVolume::FromMM(aabbMin,aabbMax); + proxy->stage = m_stageCurrent; + proxy->m_uniqueId = objectId; + proxy->leaf = m_sets[0].insert(aabb,proxy); + b3ListAppend(proxy,m_stageRoots[m_stageCurrent]); + if(!m_deferedcollide) + { + b3DbvtTreeCollider 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 b3DynamicBvhBroadphase::destroyProxy( b3BroadphaseProxy* absproxy, + b3Dispatcher* dispatcher) +{ + b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy; + if(proxy->stage==STAGECOUNT) + m_sets[1].remove(proxy->leaf); + else + m_sets[0].remove(proxy->leaf); + b3ListRemove(proxy,m_stageRoots[proxy->stage]); + m_paircache->removeOverlappingPairsContainingProxy(proxy->getUid(),dispatcher); + + m_needcleanup=true; +} + +void b3DynamicBvhBroadphase::getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const +{ + const b3DbvtProxy* proxy=&m_proxies[objectId]; + aabbMin = proxy->m_aabbMin; + aabbMax = proxy->m_aabbMax; +} +/* +void b3DynamicBvhBroadphase::getAabb(b3BroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const +{ + b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy; + aabbMin = proxy->m_aabbMin; + aabbMax = proxy->m_aabbMax; +} +*/ + + +struct BroadphaseRayTester : b3DynamicBvh::ICollide +{ + b3BroadphaseRayCallback& m_rayCallback; + BroadphaseRayTester(b3BroadphaseRayCallback& orgCallback) + :m_rayCallback(orgCallback) + { + } + void Process(const b3DbvtNode* leaf) + { + b3DbvtProxy* proxy=(b3DbvtProxy*)leaf->data; + m_rayCallback.process(proxy); + } +}; + +void b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback,const b3Vector3& aabbMin,const b3Vector3& 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); + +} + + +struct BroadphaseAabbTester : b3DynamicBvh::ICollide +{ + b3BroadphaseAabbCallback& m_aabbCallback; + BroadphaseAabbTester(b3BroadphaseAabbCallback& orgCallback) + :m_aabbCallback(orgCallback) + { + } + void Process(const b3DbvtNode* leaf) + { + b3DbvtProxy* proxy=(b3DbvtProxy*)leaf->data; + m_aabbCallback.process(proxy); + } +}; + +void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3BroadphaseAabbCallback& aabbCallback) +{ + BroadphaseAabbTester callback(aabbCallback); + + const B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds=b3DbvtVolume::FromMM(aabbMin,aabbMax); + //process all children, that overlap with the given AABB bounds + m_sets[0].collideTV(m_sets[0].m_root,bounds,callback); + m_sets[1].collideTV(m_sets[1].m_root,bounds,callback); + +} + + + +// +void b3DynamicBvhBroadphase::setAabb(int objectId, + const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + b3Dispatcher* /*dispatcher*/) +{ + b3DbvtProxy* proxy=&m_proxies[objectId]; +// b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy; + B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax); +#if B3_DBVT_BP_PREVENTFALSEUPDATE + if(b3NotEqual(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(b3Intersect(proxy->leaf->volume,aabb)) + {/* Moving */ + + const b3Vector3 delta=aabbMin-proxy->m_aabbMin; + b3Vector3 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 B3_DBVT_BP_MARGIN + m_sets[0].update(proxy->leaf,aabb,velocity,B3_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; + } + } + b3ListRemove(proxy,m_stageRoots[proxy->stage]); + proxy->m_aabbMin = aabbMin; + proxy->m_aabbMax = aabbMax; + proxy->stage = m_stageCurrent; + b3ListAppend(proxy,m_stageRoots[m_stageCurrent]); + if(docollide) + { + m_needcleanup=true; + if(!m_deferedcollide) + { + b3DbvtTreeCollider 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 b3DynamicBvhBroadphase::setAabbForceUpdate( b3BroadphaseProxy* absproxy, + const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + b3Dispatcher* /*dispatcher*/) +{ + b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy; + B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax); + 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; + /* Teleporting */ + m_sets[0].update(proxy->leaf,aabb); + ++m_updates_done; + docollide=true; + } + b3ListRemove(proxy,m_stageRoots[proxy->stage]); + proxy->m_aabbMin = aabbMin; + proxy->m_aabbMax = aabbMax; + proxy->stage = m_stageCurrent; + b3ListAppend(proxy,m_stageRoots[m_stageCurrent]); + if(docollide) + { + m_needcleanup=true; + if(!m_deferedcollide) + { + b3DbvtTreeCollider 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 b3DynamicBvhBroadphase::calculateOverlappingPairs(b3Dispatcher* dispatcher) +{ + collide(dispatcher); +#if B3_DBVT_BP_PROFILE + if(0==(m_pid%B3_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/B3_DBVT_BP_PROFILING_RATE); + printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/B3_DBVT_BP_PROFILING_RATE); + printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/B3_DBVT_BP_PROFILING_RATE); + printf("total: %uus\r\n",total/B3_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)/B3_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)*B3_DBVT_BP_PROFILING_RATE)); + b3Clear(m_profiling); + m_clock.reset(); + } +#endif + + performDeferredRemoval(dispatcher); + +} + +void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher) +{ + + if (m_paircache->hasDeferredRemoval()) + { + + b3BroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray(); + + //perform a sort, to find duplicates and to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(b3BroadphasePairSortPredicate()); + + int invalidPair = 0; + + + int i; + + b3BroadphasePair previousPair = b3MakeBroadphasePair(-1,-1); + + + + for (i=0;ileaf->volume,pb->leaf->volume); + + if (hasOverlap) + { + needsRemoval = false; + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + } + + if (needsRemoval) + { + m_paircache->cleanOverlappingPair(pair,dispatcher); + + pair.x = -1; + pair.y = -1; + invalidPair++; + } + + } + + //perform a sort, to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(b3BroadphasePairSortPredicate()); + overlappingPairArray.resize(overlappingPairArray.size() - invalidPair); + } +} + +// +void b3DynamicBvhBroadphase::collide(b3Dispatcher* 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;igetNumOverlappingPairs();i++) + { + printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(), + getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid()); + } + printf("\n"); + } +*/ + + + + b3SPC(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=b3Max(0,m_fixedleft-count); + } + /* dynamic -> fixed set */ + m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; + b3DbvtProxy* current=m_stageRoots[m_stageCurrent]; + if(current) + { + b3DbvtTreeCollider collider(this); + do { + b3DbvtProxy* next=current->links[1]; + b3ListRemove(current,m_stageRoots[current->stage]); + b3ListAppend(current,m_stageRoots[STAGECOUNT]); +#if B3_DBVT_BP_ACCURATESLEEPING + m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); + collider.proxy=current; + b3DynamicBvh::collideTV(m_sets[0].m_root,current->aabb,collider); + b3DynamicBvh::collideTV(m_sets[1].m_root,current->aabb,collider); +#endif + m_sets[0].remove(current->leaf); + B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) curAabb=b3DbvtVolume::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 */ + { + b3DbvtTreeCollider collider(this); + if(m_deferedcollide) + { + b3SPC(m_profiling.m_fdcollide); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider); + } + if(m_deferedcollide) + { + b3SPC(m_profiling.m_ddcollide); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider); + } + } + /* clean up */ + if(m_needcleanup) + { + b3SPC(m_profiling.m_cleanup); + b3BroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); + if(pairs.size()>0) + { + + int ni=b3Min(pairs.size(),b3Max(m_newpairs,(pairs.size()*m_cupdates)/100)); + for(int i=0;ileaf->volume,pb->leaf->volume)) + { +#if B3_DBVT_BP_SORTPAIRS + if(pa->m_uniqueId>pb->m_uniqueId) + b3Swap(pa,pb); +#endif + m_paircache->removeOverlappingPair(pa->getUid(),pb->getUid(),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/(b3Scalar)m_updates_call; } + else + { m_updates_ratio=0; } + m_updates_done/=2; + m_updates_call/=2; +} + +// +void b3DynamicBvhBroadphase::optimize() +{ + m_sets[0].optimizeTopDown(); + m_sets[1].optimizeTopDown(); +} + +// +b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache() +{ + return(m_paircache); +} + +// +const b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache() const +{ + return(m_paircache); +} + +// +void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const +{ + + B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds; + + if(!m_sets[0].empty()) + if(!m_sets[1].empty()) b3Merge( 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=b3DbvtVolume::FromCR(b3MakeVector3(0,0,0),0); + aabbMin=bounds.Mins(); + aabbMax=bounds.Maxs(); +} + +void b3DynamicBvhBroadphase::resetPool(b3Dispatcher* 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_pid = 0; + m_cid = 0; + for(int i=0;i<=STAGECOUNT;++i) + { + m_stageRoots[i]=0; + } + } +} + +// +void b3DynamicBvhBroadphase::printStats() +{} + +// +#if B3_DBVT_BP_ENABLE_BENCHMARK + +struct b3BroadphaseBenchmark +{ + struct Experiment + { + const char* name; + int object_count; + int update_count; + int spawn_count; + int iterations; + b3Scalar speed; + b3Scalar amplitude; + }; + struct Object + { + b3Vector3 center; + b3Vector3 extents; + b3BroadphaseProxy* proxy; + b3Scalar time; + void update(b3Scalar speed,b3Scalar amplitude,b3BroadphaseInterface* pbi) + { + time += speed; + center[0] = b3Cos(time*(b3Scalar)2.17)*amplitude+ + b3Sin(time)*amplitude/2; + center[1] = b3Cos(time*(b3Scalar)1.38)*amplitude+ + b3Sin(time)*amplitude; + center[2] = b3Sin(time*(b3Scalar)0.777)*amplitude; + pbi->setAabb(proxy,center-extents,center+extents,0); + } + }; + static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } + static b3Scalar UnitRand() { return(UnsignedRand(16384)/(b3Scalar)16384); } + static void OutputTime(const char* name,b3Clock& c,unsigned count=0) + { + const unsigned long us=c.getTimeMicroseconds(); + const unsigned long ms=(us+500)/1000; + const b3Scalar sec=us/(b3Scalar)(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 b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface* pbi) +{ + static const b3BroadphaseBenchmark::Experiment experiments[]= + { + {"1024o.10%",1024,10,0,8192,(b3Scalar)0.005,(b3Scalar)100}, + /*{"4096o.10%",4096,10,0,8192,(b3Scalar)0.005,(b3Scalar)100}, + {"8192o.10%",8192,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},*/ + }; + static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]); + b3AlignedObjectArray objects; + b3Clock wallclock; + /* Begin */ + for(int iexp=0;iexpcenter[0]=b3BroadphaseBenchmark::UnitRand()*50; + po->center[1]=b3BroadphaseBenchmark::UnitRand()*50; + po->center[2]=b3BroadphaseBenchmark::UnitRand()*50; + po->extents[0]=b3BroadphaseBenchmark::UnitRand()*2+2; + po->extents[1]=b3BroadphaseBenchmark::UnitRand()*2+2; + po->extents[2]=b3BroadphaseBenchmark::UnitRand()*2+2; + po->time=b3BroadphaseBenchmark::UnitRand()*2000; + po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); + objects.push_back(po); + } + b3BroadphaseBenchmark::OutputTime("\tInitialization",wallclock); + /* First update */ + wallclock.reset(); + for(int i=0;iupdate(speed,amplitude,pbi); + } + b3BroadphaseBenchmark::OutputTime("\tFirst update",wallclock); + /* Updates */ + wallclock.reset(); + for(int i=0;iupdate(speed,amplitude,pbi); + } + pbi->calculateOverlappingPairs(0); + } + b3BroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); + /* Clean up */ + wallclock.reset(); + for(int i=0;idestroyProxy(objects[i]->proxy,0); + delete objects[i]; + } + objects.resize(0); + b3BroadphaseBenchmark::OutputTime("\tRelease",wallclock); + } + +} +#else +/*void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface*) +{} +*/ +#endif + +#if B3_DBVT_BP_PROFILE +#undef b3SPC +#endif + diff --git a/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h new file mode 100644 index 000000000000..7ac085d90cda --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h @@ -0,0 +1,206 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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. +*/ + +///b3DynamicBvhBroadphase implementation by Nathanael Presson +#ifndef B3_DBVT_BROADPHASE_H +#define B3_DBVT_BROADPHASE_H + +#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h" +#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +#include "b3BroadphaseCallback.h" + +// +// Compile time config +// + +#define B3_DBVT_BP_PROFILE 0 +//#define B3_DBVT_BP_SORTPAIRS 1 +#define B3_DBVT_BP_PREVENTFALSEUPDATE 0 +#define B3_DBVT_BP_ACCURATESLEEPING 0 +#define B3_DBVT_BP_ENABLE_BENCHMARK 0 +#define B3_DBVT_BP_MARGIN (b3Scalar)0.05 + +#if B3_DBVT_BP_PROFILE +#define B3_DBVT_BP_PROFILING_RATE 256 + +#endif + + + + +B3_ATTRIBUTE_ALIGNED16(struct) b3BroadphaseProxy +{ + +B3_DECLARE_ALIGNED_ALLOCATOR(); + + ///optional filtering to cull potential collisions + enum CollisionFilterGroups + { + DefaultFilter = 1, + StaticFilter = 2, + KinematicFilter = 4, + DebrisFilter = 8, + SensorTrigger = 16, + CharacterFilter = 32, + AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger + }; + + //Usually the client b3CollisionObject or Rigidbody class + void* m_clientObject; + int m_collisionFilterGroup; + int m_collisionFilterMask; + int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. + + b3Vector3 m_aabbMin; + b3Vector3 m_aabbMax; + + B3_FORCE_INLINE int getUid() const + { + return m_uniqueId; + } + + //used for memory pools + b3BroadphaseProxy() :m_clientObject(0) + { + } + + b3BroadphaseProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask) + :m_clientObject(userPtr), + m_collisionFilterGroup(collisionFilterGroup), + m_collisionFilterMask(collisionFilterMask), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax) + { + } +}; + + + + + +// +// b3DbvtProxy +// +struct b3DbvtProxy : b3BroadphaseProxy +{ + /* Fields */ + //b3DbvtAabbMm aabb; + b3DbvtNode* leaf; + b3DbvtProxy* links[2]; + int stage; + /* ctor */ + + explicit b3DbvtProxy() {} + b3DbvtProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask) : + b3BroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask) + { + links[0]=links[1]=0; + } +}; + +typedef b3AlignedObjectArray b3DbvtProxyArray; + +///The b3DynamicBvhBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see b3DynamicBvh). +///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other. +///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases b3AxisSweep3 and b332BitAxisSweep3. +struct b3DynamicBvhBroadphase +{ + /* Config */ + enum { + DYNAMIC_SET = 0, /* Dynamic set index */ + FIXED_SET = 1, /* Fixed set index */ + STAGECOUNT = 2 /* Number of stages */ + }; + /* Fields */ + b3DynamicBvh m_sets[2]; // Dbvt sets + b3DbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list + + b3AlignedObjectArray m_proxies; + b3OverlappingPairCache* m_paircache; // Pair cache + b3Scalar m_prediction; // Velocity prediction + int m_stageCurrent; // Current stage + int m_fupdates; // % of fixed updates per frame + int m_dupdates; // % of dynamic updates per frame + int m_cupdates; // % of cleanup updates per frame + int m_newpairs; // Number of pairs created + int m_fixedleft; // Fixed optimization left + unsigned m_updates_call; // Number of updates call + unsigned m_updates_done; // Number of updates done + b3Scalar m_updates_ratio; // m_updates_done/m_updates_call + int m_pid; // Parse id + int m_cid; // Cleanup index + bool m_releasepaircache; // Release pair cache on delete + bool m_deferedcollide; // Defere dynamic/static collision to collide call + bool m_needcleanup; // Need to run cleanup? +#if B3_DBVT_BP_PROFILE + b3Clock m_clock; + struct { + unsigned long m_total; + unsigned long m_ddcollide; + unsigned long m_fdcollide; + unsigned long m_cleanup; + unsigned long m_jobcount; + } m_profiling; +#endif + /* Methods */ + b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache=0); + virtual ~b3DynamicBvhBroadphase(); + void collide(b3Dispatcher* dispatcher); + void optimize(); + + /* b3BroadphaseInterface Implementation */ + b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int objectIndex,void* userPtr, int collisionFilterGroup, int collisionFilterMask); + virtual void destroyProxy(b3BroadphaseProxy* proxy,b3Dispatcher* dispatcher); + virtual void setAabb(int objectId,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* dispatcher); + virtual void rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin=b3MakeVector3(0,0,0), const b3Vector3& aabbMax = b3MakeVector3(0,0,0)); + virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback); + + //virtual void getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const; + virtual void getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const; + virtual void calculateOverlappingPairs(b3Dispatcher* dispatcher=0); + virtual b3OverlappingPairCache* getOverlappingPairCache(); + virtual const b3OverlappingPairCache* getOverlappingPairCache() const; + virtual void getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const; + virtual void printStats(); + + + ///reset broadphase internal structures, to ensure determinism/reproducability + virtual void resetPool(b3Dispatcher* dispatcher); + + void performDeferredRemoval(b3Dispatcher* dispatcher); + + void setVelocityPrediction(b3Scalar prediction) + { + m_prediction = prediction; + } + b3Scalar getVelocityPrediction() const + { + return m_prediction; + } + + ///this setAabbForceUpdate is similar to setAabb but always forces the aabb update. + ///it is not part of the b3BroadphaseInterface but specific to b3DynamicBvhBroadphase. + ///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see + ///http://code.google.com/p/bullet/issues/detail?id=223 + void setAabbForceUpdate( b3BroadphaseProxy* absproxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* /*dispatcher*/); + + //static void benchmark(b3BroadphaseInterface*); + + +}; + +#endif diff --git a/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h new file mode 100644 index 000000000000..39bf27de3e36 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h @@ -0,0 +1,72 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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. +*/ + +#ifndef B3_OVERLAPPING_PAIR_H +#define B3_OVERLAPPING_PAIR_H + +#include "Bullet3Common/shared/b3Int4.h" + +#define B3_NEW_PAIR_MARKER -1 +#define B3_REMOVED_PAIR_MARKER -2 + +typedef b3Int4 b3BroadphasePair; + +inline b3Int4 b3MakeBroadphasePair(int xx,int yy) +{ + b3Int4 pair; + + if (xx < yy) + { + pair.x = xx; + pair.y = yy; + } + else + { + pair.x = yy; + pair.y = xx; + } + pair.z = B3_NEW_PAIR_MARKER; + pair.w = B3_NEW_PAIR_MARKER; + return pair; +} + +/*struct b3BroadphasePair : public b3Int4 +{ + explicit b3BroadphasePair(){} + +}; +*/ + +class b3BroadphasePairSortPredicate +{ + public: + + bool operator() ( const b3BroadphasePair& a, const b3BroadphasePair& b ) const + { + const int uidA0 = a.x; + const int uidB0 = b.x; + const int uidA1 = a.y; + const int uidB1 = b.y; + return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1); + } +}; + +B3_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePair& b) +{ + return (a.x == b.x ) && (a.y == b.y ); +} + +#endif //B3_OVERLAPPING_PAIR_H + diff --git a/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp new file mode 100644 index 000000000000..e4bda61624ad --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp @@ -0,0 +1,638 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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 "b3OverlappingPairCache.h" + +//#include "b3Dispatcher.h" +//#include "b3CollisionAlgorithm.h" +#include "Bullet3Geometry/b3AabbUtil.h" + +#include + +int b3g_overlappingPairs = 0; +int b3g_removePairs =0; +int b3g_addedPairs =0; +int b3g_findPairs =0; + + + + +b3HashedOverlappingPairCache::b3HashedOverlappingPairCache(): + m_overlapFilterCallback(0) +//, m_blockedForChanges(false) +{ + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); + growTables(); +} + + + + +b3HashedOverlappingPairCache::~b3HashedOverlappingPairCache() +{ +} + + + +void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher) +{ +/* if (pair.m_algorithm) + { + { + pair.m_algorithm->~b3CollisionAlgorithm(); + dispatcher->freeCollisionAlgorithm(pair.m_algorithm); + pair.m_algorithm=0; + } + } + */ + +} + + + + +void b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher) +{ + + class CleanPairCallback : public b3OverlapCallback + { + int m_cleanProxy; + b3OverlappingPairCache* m_pairCache; + b3Dispatcher* m_dispatcher; + + public: + CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher) + :m_cleanProxy(cleanProxy), + m_pairCache(pairCache), + m_dispatcher(dispatcher) + { + } + virtual bool processOverlap(b3BroadphasePair& pair) + { + if ((pair.x == m_cleanProxy) || + (pair.y == m_cleanProxy)) + { + m_pairCache->cleanOverlappingPair(pair,m_dispatcher); + } + return false; + } + + }; + + CleanPairCallback cleanPairs(proxy,this,dispatcher); + + processAllOverlappingPairs(&cleanPairs,dispatcher); + +} + + + + +void b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher) +{ + + class RemovePairCallback : public b3OverlapCallback + { + int m_obsoleteProxy; + + public: + RemovePairCallback(int obsoleteProxy) + :m_obsoleteProxy(obsoleteProxy) + { + } + virtual bool processOverlap(b3BroadphasePair& pair) + { + return ((pair.x == m_obsoleteProxy) || + (pair.y == m_obsoleteProxy)); + } + + }; + + + RemovePairCallback removeCallback(proxy); + + processAllOverlappingPairs(&removeCallback,dispatcher); +} + + + + + +b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1) +{ + b3g_findPairs++; + if(proxy0 >proxy1) + b3Swap(proxy0,proxy1); + int proxyId1 = proxy0; + int proxyId2 = proxy1; + + /*if (proxyId1 > proxyId2) + b3Swap(proxyId1, proxyId2);*/ + + int hash = static_cast(getHash(static_cast(proxyId1), static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + + if (hash >= m_hashTable.size()) + { + return NULL; + } + + int index = m_hashTable[hash]; + while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) + { + index = m_next[index]; + } + + if (index == B3_NULL_PAIR) + { + return NULL; + } + + b3Assert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; +} + +//#include + +void b3HashedOverlappingPairCache::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] = B3_NULL_PAIR; + } + for (i = 0; i < newCapacity; ++i) + { + m_next[i] = B3_NULL_PAIR; + } + + for(i=0;i proxyId2) + b3Swap(proxyId1, proxyId2);*/ + int hashValue = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + m_next[i] = m_hashTable[hashValue]; + m_hashTable[hashValue] = i; + } + + + } +} + +b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1) +{ + if(proxy0>proxy1) + b3Swap(proxy0,proxy1); + int proxyId1 = proxy0; + int proxyId2 = proxy1; + + /*if (proxyId1 > proxyId2) + b3Swap(proxyId1, proxyId2);*/ + + int hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + + + b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); + if (pair != NULL) + { + return pair; + } + /*for(int i=0;i%u\r\n",proxyId1,proxyId2); + internalFindPair(proxy0, proxy1, hash); + } + }*/ + int count = m_overlappingPairArray.size(); + int oldCapacity = m_overlappingPairArray.capacity(); + pair = &m_overlappingPairArray.expandNonInitializing(); + + //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(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + } + + *pair = b3MakeBroadphasePair(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* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1,b3Dispatcher* dispatcher) +{ + b3g_removePairs++; + if(proxy0>proxy1) + b3Swap(proxy0,proxy1); + int proxyId1 = proxy0; + int proxyId2 = proxy1; + + /*if (proxyId1 > proxyId2) + b3Swap(proxyId1, proxyId2);*/ + + int hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + + b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); + if (pair == NULL) + { + return 0; + } + + cleanOverlappingPair(*pair,dispatcher); + + + + int pairIndex = int(pair - &m_overlappingPairArray[0]); + b3Assert(pairIndex < m_overlappingPairArray.size()); + + // Remove the pair from the hash table. + int index = m_hashTable[hash]; + b3Assert(index != B3_NULL_PAIR); + + int previous = B3_NULL_PAIR; + while (index != pairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != B3_NULL_PAIR) + { + b3Assert(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 0; + } + + // Remove the last pair from the hash table. + const b3BroadphasePair* last = &m_overlappingPairArray[lastPairIndex]; + /* missing swap here too, Nat. */ + int lastHash = static_cast(getHash(static_cast(last->x), static_cast(last->y)) & (m_overlappingPairArray.capacity()-1)); + + index = m_hashTable[lastHash]; + b3Assert(index != B3_NULL_PAIR); + + previous = B3_NULL_PAIR; + while (index != lastPairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != B3_NULL_PAIR) + { + b3Assert(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 0; +} +//#include + +void b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher) +{ + + int i; + +// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size()); + for (i=0;iprocessOverlap(*pair)) + { + removeOverlappingPair(pair->x,pair->y,dispatcher); + + b3g_overlappingPairs--; + } else + { + i++; + } + } +} + + + + + +void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher) +{ + ///need to keep hashmap in sync with pair address, so rebuild all + b3BroadphasePairArray tmpPairs; + int i; + for (i=0;iremoveOverlappingPair(proxy0, proxy1,dispatcher); + + m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1); + m_overlappingPairArray.pop_back(); + return 0; + } + } + + return 0; +} + + + + + + + + +b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0,int proxy1) +{ + //don't add overlap with own + b3Assert(proxy0 != proxy1); + + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + b3BroadphasePair* pair = &m_overlappingPairArray.expandNonInitializing(); + *pair = b3MakeBroadphasePair(proxy0,proxy1); + + + b3g_overlappingPairs++; + b3g_addedPairs++; + +// 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 + b3BroadphasePair* b3SortedOverlappingPairCache::findPair(int proxy0,int proxy1) +{ + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0,proxy1); + int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair); + + if (findIndex < m_overlappingPairArray.size()) + { + //b3Assert(it != m_overlappingPairSet.end()); + b3BroadphasePair* pair = &m_overlappingPairArray[findIndex]; + return pair; + } + return 0; +} + + + + + + + + + + +//#include + +void b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher) +{ + + int i; + + for (i=0;iprocessOverlap(*pair)) + { + cleanOverlappingPair(*pair,dispatcher); + pair->x = -1; + pair->y = -1; + m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + m_overlappingPairArray.pop_back(); + b3g_overlappingPairs--; + } else + { + i++; + } + } +} + + + + +b3SortedOverlappingPairCache::b3SortedOverlappingPairCache(): + m_blockedForChanges(false), + m_hasDeferredRemoval(true), + m_overlapFilterCallback(0) + +{ + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); +} + +b3SortedOverlappingPairCache::~b3SortedOverlappingPairCache() +{ +} + +void b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher) +{ +/* if (pair.m_algorithm) + { + { + pair.m_algorithm->~b3CollisionAlgorithm(); + dispatcher->freeCollisionAlgorithm(pair.m_algorithm); + pair.m_algorithm=0; + b3g_removePairs--; + } + } + */ +} + + +void b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher) +{ + + class CleanPairCallback : public b3OverlapCallback + { + int m_cleanProxy; + b3OverlappingPairCache* m_pairCache; + b3Dispatcher* m_dispatcher; + + public: + CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher) + :m_cleanProxy(cleanProxy), + m_pairCache(pairCache), + m_dispatcher(dispatcher) + { + } + virtual bool processOverlap(b3BroadphasePair& pair) + { + if ((pair.x == m_cleanProxy) || + (pair.y == m_cleanProxy)) + { + m_pairCache->cleanOverlappingPair(pair,m_dispatcher); + } + return false; + } + + }; + + CleanPairCallback cleanPairs(proxy,this,dispatcher); + + processAllOverlappingPairs(&cleanPairs,dispatcher); + +} + + +void b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher) +{ + + class RemovePairCallback : public b3OverlapCallback + { + int m_obsoleteProxy; + + public: + RemovePairCallback(int obsoleteProxy) + :m_obsoleteProxy(obsoleteProxy) + { + } + virtual bool processOverlap(b3BroadphasePair& pair) + { + return ((pair.x == m_obsoleteProxy) || + (pair.y == m_obsoleteProxy)); + } + + }; + + RemovePairCallback removeCallback(proxy); + + processAllOverlappingPairs(&removeCallback,dispatcher); +} + +void b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher) +{ + //should already be sorted +} + diff --git a/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h new file mode 100644 index 000000000000..f67eb676f1d6 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h @@ -0,0 +1,474 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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. +*/ + +#ifndef B3_OVERLAPPING_PAIR_CACHE_H +#define B3_OVERLAPPING_PAIR_CACHE_H + +#include "Bullet3Common/shared/b3Int2.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +class b3Dispatcher; +#include "b3OverlappingPair.h" + + + +typedef b3AlignedObjectArray b3BroadphasePairArray; + +struct b3OverlapCallback +{ + virtual ~b3OverlapCallback() + {} + //return true for deletion of the pair + virtual bool processOverlap(b3BroadphasePair& pair) = 0; + +}; + +struct b3OverlapFilterCallback +{ + virtual ~b3OverlapFilterCallback() + {} + // return true when pairs need collision + virtual bool needBroadphaseCollision(int proxy0,int proxy1) const = 0; +}; + + + + + + + +extern int b3g_removePairs; +extern int b3g_addedPairs; +extern int b3g_findPairs; + +const int B3_NULL_PAIR=0xffffffff; + +///The b3OverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the b3BroadphaseInterface broadphases. +///The b3HashedOverlappingPairCache and b3SortedOverlappingPairCache classes are two implementations. +class b3OverlappingPairCache +{ +public: + virtual ~b3OverlappingPairCache() {} // this is needed so we can get to the derived class destructor + + virtual b3BroadphasePair* getOverlappingPairArrayPtr() = 0; + + virtual const b3BroadphasePair* getOverlappingPairArrayPtr() const = 0; + + virtual b3BroadphasePairArray& getOverlappingPairArray() = 0; + + virtual void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher) = 0; + + virtual int getNumOverlappingPairs() const = 0; + + virtual void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher) = 0; + + virtual void setOverlapFilterCallback(b3OverlapFilterCallback* callback) = 0; + + virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher) = 0; + + virtual b3BroadphasePair* findPair(int proxy0, int proxy1) = 0; + + virtual bool hasDeferredRemoval() = 0; + + //virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)=0; + + virtual b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1)=0; + virtual void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher)=0; + virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/)=0; + + virtual void sortOverlappingPairs(b3Dispatcher* dispatcher) = 0; + + +}; + +/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com +class b3HashedOverlappingPairCache : public b3OverlappingPairCache +{ + b3BroadphasePairArray m_overlappingPairArray; + b3OverlapFilterCallback* m_overlapFilterCallback; +// bool m_blockedForChanges; + + +public: + b3HashedOverlappingPairCache(); + virtual ~b3HashedOverlappingPairCache(); + + + virtual void removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher); + + virtual void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher); + + B3_FORCE_INLINE bool needsBroadphaseCollision(int proxy0,int proxy1) const + { + if (m_overlapFilterCallback) + return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + + bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + //collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + return collides; + } + + // Add a pair and return the new pair. If the pair already exists, + // no new pair is created and the old one is returned. + virtual b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1) + { + b3g_addedPairs++; + + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + return internalAddPair(proxy0,proxy1); + } + + + + void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher); + + + virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher); + + virtual b3BroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + + const b3BroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + + b3BroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const b3BroadphasePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + + void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher); + + + + b3BroadphasePair* findPair(int proxy0, int proxy1); + + int GetCount() const { return m_overlappingPairArray.size(); } +// b3BroadphasePair* GetPairs() { return m_pairs; } + + b3OverlapFilterCallback* getOverlapFilterCallback() + { + return m_overlapFilterCallback; + } + + void setOverlapFilterCallback(b3OverlapFilterCallback* callback) + { + m_overlapFilterCallback = callback; + } + + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } +private: + + b3BroadphasePair* internalAddPair(int proxy0,int proxy1); + + void growTables(); + + B3_FORCE_INLINE bool equalsPair(const b3BroadphasePair& pair, int proxyId1, int proxyId2) + { + return pair.x == proxyId1 && pair.y == proxyId2; + } + + /* + // Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm + // This assumes proxyId1 and proxyId2 are 16-bit. + B3_FORCE_INLINE int getHash(int proxyId1, int proxyId2) + { + int key = (proxyId2 << 16) | proxyId1; + key = ~key + (key << 15); + key = key ^ (key >> 12); + key = key + (key << 2); + key = key ^ (key >> 4); + key = key * 2057; + key = key ^ (key >> 16); + return key; + } + */ + + + + B3_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2) + { + int key = static_cast(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16)); + // Thomas Wang's hash + + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return static_cast(key); + } + + + + + + B3_FORCE_INLINE b3BroadphasePair* internalFindPair(int proxy0, int proxy1, int hash) + { + int proxyId1 = proxy0; + int proxyId2 = proxy1; + #if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat. + if (proxyId1 > proxyId2) + b3Swap(proxyId1, proxyId2); + #endif + + int index = m_hashTable[hash]; + + while( index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) + { + index = m_next[index]; + } + + if ( index == B3_NULL_PAIR ) + { + return NULL; + } + + b3Assert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; + } + + virtual bool hasDeferredRemoval() + { + return false; + } + +/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback) + { + m_ghostPairCallback = ghostPairCallback; + } + */ + + virtual void sortOverlappingPairs(b3Dispatcher* dispatcher); + + +protected: + + b3AlignedObjectArray m_hashTable; + b3AlignedObjectArray m_next; +// b3OverlappingPairCallback* m_ghostPairCallback; + +}; + + + + +///b3SortedOverlappingPairCache maintains the objects with overlapping AABB +///Typically managed by the Broadphase, Axis3Sweep or b3SimpleBroadphase +class b3SortedOverlappingPairCache : public b3OverlappingPairCache +{ + protected: + //avoid brute-force finding all the time + b3BroadphasePairArray m_overlappingPairArray; + + //during the dispatch, check that user doesn't destroy/create proxy + bool m_blockedForChanges; + + ///by default, do the removal during the pair traversal + bool m_hasDeferredRemoval; + + //if set, use the callback instead of the built in filter in needBroadphaseCollision + b3OverlapFilterCallback* m_overlapFilterCallback; + +// b3OverlappingPairCallback* m_ghostPairCallback; + + public: + + b3SortedOverlappingPairCache(); + virtual ~b3SortedOverlappingPairCache(); + + virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher); + + void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher); + + void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher); + + b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1); + + b3BroadphasePair* findPair(int proxy0,int proxy1); + + + void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher); + + virtual void removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher); + + + inline bool needsBroadphaseCollision(int proxy0,int proxy1) const + { + if (m_overlapFilterCallback) + return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + + bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + //collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + return collides; + } + + b3BroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const b3BroadphasePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + + + + + b3BroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + + const b3BroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } + + b3OverlapFilterCallback* getOverlapFilterCallback() + { + return m_overlapFilterCallback; + } + + void setOverlapFilterCallback(b3OverlapFilterCallback* callback) + { + m_overlapFilterCallback = callback; + } + + virtual bool hasDeferredRemoval() + { + return m_hasDeferredRemoval; + } + +/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback) + { + m_ghostPairCallback = ghostPairCallback; + } + */ + virtual void sortOverlappingPairs(b3Dispatcher* dispatcher); + + +}; + + + +///b3NullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing. +class b3NullPairCache : public b3OverlappingPairCache +{ + + b3BroadphasePairArray m_overlappingPairArray; + +public: + + virtual b3BroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + const b3BroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + b3BroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + virtual void cleanOverlappingPair(b3BroadphasePair& /*pair*/,b3Dispatcher* /*dispatcher*/) + { + + } + + virtual int getNumOverlappingPairs() const + { + return 0; + } + + virtual void cleanProxyFromPairs(int /*proxy*/,b3Dispatcher* /*dispatcher*/) + { + + } + + virtual void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/) + { + } + + virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* /*dispatcher*/) + { + } + + virtual b3BroadphasePair* findPair(int /*proxy0*/, int /*proxy1*/) + { + return 0; + } + + virtual bool hasDeferredRemoval() + { + return true; + } + +// virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* /* ghostPairCallback */) +// { +// +// } + + virtual b3BroadphasePair* addOverlappingPair(int /*proxy0*/,int /*proxy1*/) + { + return 0; + } + + virtual void* removeOverlappingPair(int /*proxy0*/,int /*proxy1*/,b3Dispatcher* /*dispatcher*/) + { + return 0; + } + + virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/) + { + } + + virtual void sortOverlappingPairs(b3Dispatcher* dispatcher) + { + (void) dispatcher; + } + + +}; + + +#endif //B3_OVERLAPPING_PAIR_CACHE_H + + diff --git a/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h new file mode 100644 index 000000000000..7f9bf990bfc2 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h @@ -0,0 +1,59 @@ + +#ifndef B3_AABB_H +#define B3_AABB_H + + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Common/shared/b3Mat3x3.h" + +typedef struct b3Aabb b3Aabb_t; + +struct b3Aabb +{ + union + { + float m_min[4]; + b3Float4 m_minVec; + int m_minIndices[4]; + }; + union + { + float m_max[4]; + b3Float4 m_maxVec; + int m_signedMaxIndices[4]; + }; +}; + +inline void b3TransformAabb2(b3Float4ConstArg localAabbMin,b3Float4ConstArg localAabbMax, float margin, + b3Float4ConstArg pos, + b3QuatConstArg orn, + b3Float4* aabbMinOut,b3Float4* aabbMaxOut) +{ + b3Float4 localHalfExtents = 0.5f*(localAabbMax-localAabbMin); + localHalfExtents+=b3MakeFloat4(margin,margin,margin,0.f); + b3Float4 localCenter = 0.5f*(localAabbMax+localAabbMin); + b3Mat3x3 m; + m = b3QuatGetRotationMatrix(orn); + b3Mat3x3 abs_b = b3AbsoluteMat3x3(m); + b3Float4 center = b3TransformPoint(localCenter,pos,orn); + + b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents,b3GetRow(abs_b,0)), + b3Dot3F4(localHalfExtents,b3GetRow(abs_b,1)), + b3Dot3F4(localHalfExtents,b3GetRow(abs_b,2)), + 0.f); + *aabbMinOut = center-extent; + *aabbMaxOut = center+extent; +} + +/// conservative test for overlap between two aabbs +inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1,b3Float4ConstArg aabbMax1, + b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2) +{ + bool overlap = true; + overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap; + overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap; + overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap; + return overlap; +} + +#endif //B3_AABB_H diff --git a/extern/bullet/src/Bullet3Collision/CMakeLists.txt b/extern/bullet/src/Bullet3Collision/CMakeLists.txt new file mode 100644 index 000000000000..130095cc04ec --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/CMakeLists.txt @@ -0,0 +1,93 @@ + +INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/src +) + +SET(Bullet3Collision_SRCS + BroadPhaseCollision/b3DynamicBvh.cpp + BroadPhaseCollision/b3DynamicBvhBroadphase.cpp + BroadPhaseCollision/b3OverlappingPairCache.cpp + NarrowPhaseCollision/b3ConvexUtility.cpp + NarrowPhaseCollision/b3CpuNarrowPhase.cpp +) + +SET(Bullet3CollisionBroadPhase_HDRS + BroadPhaseCollision/b3BroadphaseCallback.h + BroadPhaseCollision/b3DynamicBvh.h + BroadPhaseCollision/b3DynamicBvhBroadphase.h + BroadPhaseCollision/b3OverlappingPair.h + BroadPhaseCollision/b3OverlappingPairCache.h +) +SET(Bullet3CollisionBroadPhaseShared_HDRS + BroadPhaseCollision/shared/b3Aabb.h +) + +SET(Bullet3CollisionNarrowPhase_HDRS + NarrowPhaseCollision/b3Config.h + NarrowPhaseCollision/b3Contact4.h + NarrowPhaseCollision/b3ConvexUtility.h + NarrowPhaseCollision/b3CpuNarrowPhase.h + NarrowPhaseCollision/b3RaycastInfo.h + NarrowPhaseCollision/b3RigidBodyCL.h +) +SET(Bullet3CollisionNarrowPhaseShared_HDRS + + NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h + NarrowPhaseCollision/shared/b3BvhTraversal.h + NarrowPhaseCollision/shared/b3ClipFaces.h + NarrowPhaseCollision/shared/b3Collidable.h + NarrowPhaseCollision/shared/b3Contact4Data.h + NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h + NarrowPhaseCollision/shared/b3ContactSphereSphere.h + NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h + NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h + NarrowPhaseCollision/shared/b3FindSeparatingAxis.h + NarrowPhaseCollision/shared/b3MprPenetration.h + NarrowPhaseCollision/shared/b3NewContactReduction.h + NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h + NarrowPhaseCollision/shared/b3ReduceContacts.h + NarrowPhaseCollision/shared/b3RigidBodyData.h + NarrowPhaseCollision/shared/b3UpdateAabbs.h +) + +SET(Bullet3Collision_HDRS + ${Bullet3CollisionBroadPhase_HDRS} + ${Bullet3CollisionBroadPhaseShared_HDRS} + ${Bullet3CollisionNarrowPhaseShared_HDRS} + ${Bullet3CollisionNarrowPhase_HDRS} +) + +ADD_LIBRARY(Bullet3Collision ${Bullet3Collision_SRCS} ${Bullet3Collision_HDRS}) +if (BUILD_SHARED_LIBS) + target_link_libraries(Bullet3Collision Bullet3Geometry) +endif () +SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES SOVERSION ${BULLET_VERSION}) + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + #FILES_MATCHING requires CMake 2.6 + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS Bullet3Collision DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS Bullet3Collision + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN +".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES PUBLIC_HEADER "${Bullet3Collision_HDRS}") + # Have to list out sub-directories manually: + #todo + #SET_PROPERTY(SOURCE ${Bullet3CollisionBroadPhase_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/BroadPhaseCollision) + + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h new file mode 100644 index 000000000000..65d4a21613a5 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h @@ -0,0 +1,41 @@ +#ifndef B3_CONFIG_H +#define B3_CONFIG_H + +struct b3Config +{ + int m_maxConvexBodies; + int m_maxConvexShapes; + int m_maxBroadphasePairs; + int m_maxContactCapacity; + int m_compoundPairCapacity; + + int m_maxVerticesPerFace; + int m_maxFacesPerShape; + int m_maxConvexVertices; + int m_maxConvexIndices; + int m_maxConvexUniqueEdges; + + int m_maxCompoundChildShapes; + + int m_maxTriConvexPairCapacity; + + b3Config() + :m_maxConvexBodies(128*1024), + m_maxVerticesPerFace(64), + m_maxFacesPerShape(12), + m_maxConvexVertices(8192), + m_maxConvexIndices(81920), + m_maxConvexUniqueEdges(8192), + m_maxCompoundChildShapes(8192), + m_maxTriConvexPairCapacity(256*1024) + { + m_maxConvexShapes = m_maxConvexBodies; + m_maxBroadphasePairs = 16*m_maxConvexBodies; + m_maxContactCapacity = m_maxBroadphasePairs; + m_compoundPairCapacity = 1024*1024; + } +}; + + +#endif//B3_CONFIG_H + diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h new file mode 100644 index 000000000000..fb2516567316 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h @@ -0,0 +1,46 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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. +*/ + +#ifndef B3_CONTACT4_H +#define B3_CONTACT4_H + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +B3_ATTRIBUTE_ALIGNED16(struct) b3Contact4 : public b3Contact4Data +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); + + int getBodyA()const {return abs(m_bodyAPtrAndSignBit);} + int getBodyB()const {return abs(m_bodyBPtrAndSignBit);} + bool isBodyAFixed()const { return m_bodyAPtrAndSignBit<0;} + bool isBodyBFixed()const { return m_bodyBPtrAndSignBit<0;} + // todo. make it safer + int& getBatchIdx() { return m_batchIdx; } + const int& getBatchIdx() const { return m_batchIdx; } + float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp/(float)0xffff); } + void setRestituitionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_restituitionCoeffCmp = (unsigned short)(c*0xffff); } + float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp/(float)0xffff); } + void setFrictionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_frictionCoeffCmp = (unsigned short)(c*0xffff); } + + //float& getNPoints() { return m_worldNormal[3]; } + int getNPoints() const { return (int) m_worldNormalOnB.w; } + + float getPenetration(int idx) const { return m_worldPosB[idx].w; } + + bool isInvalid() const { return (getBodyA()==0 || getBodyB()==0); } +}; + +#endif //B3_CONTACT4_H diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp new file mode 100644 index 000000000000..55706fa63136 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp @@ -0,0 +1,520 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + + +#include "b3ConvexUtility.h" +#include "Bullet3Geometry/b3ConvexHullComputer.h" +#include "Bullet3Geometry/b3GrahamScan2dConvexHull.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3HashMap.h" + + + + + +b3ConvexUtility::~b3ConvexUtility() +{ +} + +bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles) +{ + + + + b3ConvexHullComputer conv; + conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3),numPoints,0.f,0.f); + + b3AlignedObjectArray faceNormals; + int numFaces = conv.faces.size(); + faceNormals.resize(numFaces); + b3ConvexHullComputer* convexUtil = &conv; + + + b3AlignedObjectArray tmpFaces; + tmpFaces.resize(numFaces); + + int numVertices = convexUtil->vertices.size(); + m_vertices.resize(numVertices); + for (int p=0;pvertices[p]; + } + + + for (int i=0;ifaces[i]; + //printf("face=%d\n",face); + const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; + const b3ConvexHullComputer::Edge* edge = firstEdge; + + b3Vector3 edges[3]; + int numEdges = 0; + //compute face normals + + do + { + + int src = edge->getSourceVertex(); + tmpFaces[i].m_indices.push_back(src); + int targ = edge->getTargetVertex(); + b3Vector3 wa = convexUtil->vertices[src]; + + b3Vector3 wb = convexUtil->vertices[targ]; + b3Vector3 newEdge = wb-wa; + newEdge.normalize(); + if (numEdges<2) + edges[numEdges++] = newEdge; + + edge = edge->getNextEdgeOfFace(); + } while (edge!=firstEdge); + + b3Scalar planeEq = 1e30f; + + + if (numEdges==2) + { + faceNormals[i] = edges[0].cross(edges[1]); + faceNormals[i].normalize(); + tmpFaces[i].m_plane[0] = faceNormals[i].getX(); + tmpFaces[i].m_plane[1] = faceNormals[i].getY(); + tmpFaces[i].m_plane[2] = faceNormals[i].getZ(); + tmpFaces[i].m_plane[3] = planeEq; + + } + else + { + b3Assert(0);//degenerate? + faceNormals[i].setZero(); + } + + for (int v=0;veq) + { + planeEq=eq; + } + } + tmpFaces[i].m_plane[3] = -planeEq; + } + + //merge coplanar faces and copy them to m_polyhedron + + b3Scalar faceWeldThreshold= 0.999f; + b3AlignedObjectArray todoFaces; + for (int i=0;i coplanarFaceGroup; + int refFace = todoFaces[todoFaces.size()-1]; + + coplanarFaceGroup.push_back(refFace); + b3MyFace& faceA = tmpFaces[refFace]; + todoFaces.pop_back(); + + b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]); + for (int j=todoFaces.size()-1;j>=0;j--) + { + int i = todoFaces[j]; + b3MyFace& faceB = tmpFaces[i]; + b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]); + if (faceNormalA.dot(faceNormalB)>faceWeldThreshold) + { + coplanarFaceGroup.push_back(i); + todoFaces.remove(i); + } + } + + + bool did_merge = false; + if (coplanarFaceGroup.size()>1) + { + //do the merge: use Graham Scan 2d convex hull + + b3AlignedObjectArray orgpoints; + b3Vector3 averageFaceNormal = b3MakeVector3(0,0,0); + + for (int i=0;im_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); + + b3MyFace& face = tmpFaces[coplanarFaceGroup[i]]; + b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0],face.m_plane[1],face.m_plane[2]); + averageFaceNormal+=faceNormal; + for (int f=0;f hull; + + averageFaceNormal.normalize(); + b3GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal); + + for (int i=0;i1e-6 || fabsf(v.getY())>1e-6 || fabsf(v.getZ())>1e-6) return false; + return true; +} + +struct b3InternalVertexPair +{ + b3InternalVertexPair(short int v0,short int v1) + :m_v0(v0), + m_v1(v1) + { + if (m_v1>m_v0) + b3Swap(m_v0,m_v1); + } + short int m_v0; + short int m_v1; + int getHash() const + { + return m_v0+(m_v1<<16); + } + bool equals(const b3InternalVertexPair& other) const + { + return m_v0==other.m_v0 && m_v1==other.m_v1; + } +}; + +struct b3InternalEdge +{ + b3InternalEdge() + :m_face0(-1), + m_face1(-1) + { + } + short int m_face0; + short int m_face1; +}; + +// + +#ifdef TEST_INTERNAL_OBJECTS +bool b3ConvexUtility::testContainment() const +{ + for(int p=0;p<8;p++) + { + b3Vector3 LocalPt; + if(p==0) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]); + else if(p==1) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]); + else if(p==2) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]); + else if(p==3) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]); + else if(p==4) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]); + else if(p==5) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]); + else if(p==6) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]); + else if(p==7) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]); + + for(int i=0;i0.0f) + return false; + } + } + return true; +} +#endif + +void b3ConvexUtility::initialize() +{ + + b3HashMap edges; + + b3Scalar TotalArea = 0.0f; + + m_localCenter.setValue(0, 0, 0); + for(int i=0;im_face0>=0); + // b3Assert(edptr->m_face1<0); + edptr->m_face1 = i; + } else + { + b3InternalEdge ed; + ed.m_face0 = i; + edges.insert(vp,ed); + } + } + } + +#ifdef USE_CONNECTED_FACES + for(int i=0;im_face0>=0); + b3Assert(edptr->m_face1>=0); + + int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0; + m_faces[i].m_connectedFaces[j] = connectedFace; + } + } +#endif//USE_CONNECTED_FACES + + for(int i=0;iMaxX) MaxX = pt.getX(); + if(pt.getY()MaxY) MaxY = pt.getY(); + if(pt.getZ()MaxZ) MaxZ = pt.getZ(); + } + mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ); + mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ); + + + +// const b3Scalar r = m_radius / sqrtf(2.0f); + const b3Scalar r = m_radius / sqrtf(3.0f); + const int LargestExtent = mE.maxAxis(); + const b3Scalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f; + m_extents[0] = m_extents[1] = m_extents[2] = r; + m_extents[LargestExtent] = mE[LargestExtent]*0.5f; + bool FoundBox = false; + for(int j=0;j<1024;j++) + { + if(testContainment()) + { + FoundBox = true; + break; + } + + m_extents[LargestExtent] -= Step; + } + if(!FoundBox) + { + m_extents[0] = m_extents[1] = m_extents[2] = r; + } + else + { + // Refine the box + const b3Scalar Step = (m_radius - r)/1024.0f; + const int e0 = (1< m_indices; + b3Scalar m_plane[4]; +}; + +B3_ATTRIBUTE_ALIGNED16(class) b3ConvexUtility +{ + public: + B3_DECLARE_ALIGNED_ALLOCATOR(); + + b3Vector3 m_localCenter; + b3Vector3 m_extents; + b3Vector3 mC; + b3Vector3 mE; + b3Scalar m_radius; + + b3AlignedObjectArray m_vertices; + b3AlignedObjectArray m_faces; + b3AlignedObjectArray m_uniqueEdges; + + + b3ConvexUtility() + { + } + virtual ~b3ConvexUtility(); + + bool initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles=true); + + void initialize(); + bool testContainment() const; + + + +}; +#endif + \ No newline at end of file diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp new file mode 100644 index 000000000000..c3134b2c6594 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp @@ -0,0 +1,323 @@ +#include "b3CpuNarrowPhase.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h" + + +struct b3CpuNarrowPhaseInternalData +{ + b3AlignedObjectArray m_localShapeAABBCPU; + b3AlignedObjectArray m_collidablesCPU; + b3AlignedObjectArray m_convexData; + b3Config m_config; + + + b3AlignedObjectArray m_convexPolyhedra; + b3AlignedObjectArray m_uniqueEdges; + b3AlignedObjectArray m_convexVertices; + b3AlignedObjectArray m_convexIndices; + b3AlignedObjectArray m_convexFaces; + + b3AlignedObjectArray m_contacts; + + int m_numAcceleratedShapes; +}; + + +const b3AlignedObjectArray& b3CpuNarrowPhase::getContacts() const +{ + return m_data->m_contacts; +} + +b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) +{ + return m_data->m_collidablesCPU[collidableIndex]; +} + +const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) const +{ + return m_data->m_collidablesCPU[collidableIndex]; +} + + +b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config) +{ + m_data = new b3CpuNarrowPhaseInternalData; + m_data->m_config = config; + m_data->m_numAcceleratedShapes = 0; +} + +b3CpuNarrowPhase::~b3CpuNarrowPhase() +{ + delete m_data; +} + +void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray& pairs, b3AlignedObjectArray& aabbsWorldSpace, b3AlignedObjectArray& bodies) +{ + int nPairs = pairs.size(); + int numContacts = 0; + int maxContactCapacity = m_data->m_config.m_maxContactCapacity; + m_data->m_contacts.resize(maxContactCapacity); + + for (int i=0;im_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_SPHERE && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL) + { +// computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_SPHERE) + { +// computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); + //printf("convex-sphere\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE) + { +// computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); +// printf("convex-plane\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL) + { +// computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); +// printf("plane-convex\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) + { +// computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0], +// nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU); +// printf("convex-plane\n"); + + } + + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE) + { +// computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); +// printf("convex-plane\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) + { +// computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0], +// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); +// printf("plane-convex\n"); + + } + + if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL) + { + //printf("pairs[i].z=%d\n",pairs[i].z); + //int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies, + // m_data->m_collidablesCPU,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts); + int contactIndex = b3ContactConvexConvexSAT(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies, + m_data->m_collidablesCPU,m_data->m_convexPolyhedra,m_data->m_convexVertices,m_data->m_uniqueEdges,m_data->m_convexIndices,m_data->m_convexFaces,m_data->m_contacts,numContacts,maxContactCapacity); + + + if (contactIndex>=0) + { + pairs[i].z = contactIndex; + } +// printf("plane-convex\n"); + + } + + + } + + m_data->m_contacts.resize(numContacts); +} + +int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr) +{ + int collidableIndex = allocateCollidable(); + if (collidableIndex<0) + return collidableIndex; + + + b3Collidable& col = m_data->m_collidablesCPU[collidableIndex]; + col.m_shapeType = SHAPE_CONVEX_HULL; + col.m_shapeIndex = -1; + + + { + b3Vector3 localCenter=b3MakeVector3(0,0,0); + for (int i=0;im_vertices.size();i++) + localCenter+=utilPtr->m_vertices[i]; + localCenter*= (1.f/utilPtr->m_vertices.size()); + utilPtr->m_localCenter = localCenter; + + col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr,col); + } + + if (col.m_shapeIndex>=0) + { + b3Aabb aabb; + + b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f); + b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f); + + for (int i=0;im_vertices.size();i++) + { + myAabbMin.setMin(utilPtr->m_vertices[i]); + myAabbMax.setMax(utilPtr->m_vertices[i]); + } + aabb.m_min[0] = myAabbMin[0]; + aabb.m_min[1] = myAabbMin[1]; + aabb.m_min[2] = myAabbMin[2]; + aabb.m_minIndices[3] = 0; + + aabb.m_max[0] = myAabbMax[0]; + aabb.m_max[1] = myAabbMax[1]; + aabb.m_max[2] = myAabbMax[2]; + aabb.m_signedMaxIndices[3] = 0; + + m_data->m_localShapeAABBCPU.push_back(aabb); + + } + + return collidableIndex; +} + +int b3CpuNarrowPhase::allocateCollidable() +{ + int curSize = m_data->m_collidablesCPU.size(); + if (curSizem_config.m_maxConvexShapes) + { + m_data->m_collidablesCPU.expand(); + return curSize; + } + else + { + b3Error("allocateCollidable out-of-range %d\n",m_data->m_config.m_maxConvexShapes); + } + return -1; + +} + +int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling) +{ + b3AlignedObjectArray verts; + + unsigned char* vts = (unsigned char*) vertices; + for (int i=0;iinitializePolyhedralFeatures(&verts[0],verts.size(),merge); + } + + int collidableIndex = registerConvexHullShape(utilPtr); + + delete utilPtr; + return collidableIndex; +} + + +int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col) +{ + + m_data->m_convexData.resize(m_data->m_numAcceleratedShapes+1); + m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1); + + + b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1); + convex.mC = convexPtr->mC; + convex.mE = convexPtr->mE; + convex.m_extents= convexPtr->m_extents; + convex.m_localCenter = convexPtr->m_localCenter; + convex.m_radius = convexPtr->m_radius; + + convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size(); + int edgeOffset = m_data->m_uniqueEdges.size(); + convex.m_uniqueEdgesOffset = edgeOffset; + + m_data->m_uniqueEdges.resize(edgeOffset+convex.m_numUniqueEdges); + + //convex data here + int i; + for ( i=0;im_uniqueEdges.size();i++) + { + m_data->m_uniqueEdges[edgeOffset+i] = convexPtr->m_uniqueEdges[i]; + } + + int faceOffset = m_data->m_convexFaces.size(); + convex.m_faceOffset = faceOffset; + convex.m_numFaces = convexPtr->m_faces.size(); + + m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces); + + + for (i=0;im_faces.size();i++) + { + m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0], + convexPtr->m_faces[i].m_plane[1], + convexPtr->m_faces[i].m_plane[2], + convexPtr->m_faces[i].m_plane[3]); + + + int indexOffset = m_data->m_convexIndices.size(); + int numIndices = convexPtr->m_faces[i].m_indices.size(); + m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices; + m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset; + m_data->m_convexIndices.resize(indexOffset+numIndices); + for (int p=0;pm_convexIndices[indexOffset+p] = convexPtr->m_faces[i].m_indices[p]; + } + } + + convex.m_numVertices = convexPtr->m_vertices.size(); + int vertexOffset = m_data->m_convexVertices.size(); + convex.m_vertexOffset =vertexOffset; + + m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices); + for (int i=0;im_vertices.size();i++) + { + m_data->m_convexVertices[vertexOffset+i] = convexPtr->m_vertices[i]; + } + + (m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr; + + + + return m_data->m_numAcceleratedShapes++; +} + +const b3Aabb& b3CpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const +{ + return m_data->m_localShapeAABBCPU[collidableIndex]; +} diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h new file mode 100644 index 000000000000..528be3346de7 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h @@ -0,0 +1,105 @@ +#ifndef B3_CPU_NARROWPHASE_H +#define B3_CPU_NARROWPHASE_H + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +class b3CpuNarrowPhase +{ +protected: + + struct b3CpuNarrowPhaseInternalData* m_data; + int m_acceleratedCompanionShapeIndex; + int m_planeBodyIndex; + int m_static0Index; + + int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr,b3Collidable& col); + int registerConcaveMeshShape(b3AlignedObjectArray* vertices, b3AlignedObjectArray* indices, b3Collidable& col, const float* scaling); + +public: + + + + + b3CpuNarrowPhase(const struct b3Config& config); + + virtual ~b3CpuNarrowPhase(void); + + int registerSphereShape(float radius); + int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant); + + int registerCompoundShape(b3AlignedObjectArray* childShapes); + int registerFace(const b3Vector3& faceNormal, float faceConstant); + + int registerConcaveMesh(b3AlignedObjectArray* vertices, b3AlignedObjectArray* indices,const float* scaling); + + //do they need to be merged? + + int registerConvexHullShape(b3ConvexUtility* utilPtr); + int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling); + + //int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu); + void setObjectTransform(const float* position, const float* orientation , int bodyIndex); + + void writeAllBodiesToGpu(); + void reset(); + void readbackAllBodiesToCpu(); + bool getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const; + + void setObjectTransformCpu(float* position, float* orientation , int bodyIndex); + void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex); + + + //virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects); + virtual void computeContacts(b3AlignedObjectArray& pairs, b3AlignedObjectArray& aabbsWorldSpace, b3AlignedObjectArray& bodies); + + + + const struct b3RigidBodyData* getBodiesCpu() const; + //struct b3RigidBodyData* getBodiesCpu(); + + int getNumBodiesGpu() const; + + + int getNumBodyInertiasGpu() const; + + + const struct b3Collidable* getCollidablesCpu() const; + int getNumCollidablesGpu() const; + + + /*const struct b3Contact4* getContactsCPU() const; + + + int getNumContactsGpu() const; + */ + + const b3AlignedObjectArray& getContacts() const; + + + int getNumRigidBodies() const; + + int allocateCollidable(); + + int getStatic0Index() const + { + return m_static0Index; + } + b3Collidable& getCollidableCpu(int collidableIndex); + const b3Collidable& getCollidableCpu(int collidableIndex) const; + + const b3CpuNarrowPhaseInternalData* getInternalData() const + { + return m_data; + } + + const struct b3Aabb& getLocalSpaceAabb(int collidableIndex) const; +}; + +#endif //B3_CPU_NARROWPHASE_H + diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h new file mode 100644 index 000000000000..fba8bd07a4fd --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h @@ -0,0 +1,24 @@ + +#ifndef B3_RAYCAST_INFO_H +#define B3_RAYCAST_INFO_H + +#include "Bullet3Common/b3Vector3.h" + +B3_ATTRIBUTE_ALIGNED16(struct) b3RayInfo +{ + b3Vector3 m_from; + b3Vector3 m_to; +}; + +B3_ATTRIBUTE_ALIGNED16(struct) b3RayHit +{ + b3Scalar m_hitFraction; + int m_hitBody; + int m_hitResult1; + int m_hitResult2; + b3Vector3 m_hitPoint; + b3Vector3 m_hitNormal; +}; + +#endif //B3_RAYCAST_INFO_H + diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h new file mode 100644 index 000000000000..d58f71802f7f --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h @@ -0,0 +1,30 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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. +*/ + +#ifndef B3_RIGID_BODY_CL +#define B3_RIGID_BODY_CL + +#include "Bullet3Common/b3Scalar.h" +#include "Bullet3Common/b3Matrix3x3.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + + +inline float b3GetInvMass(const b3RigidBodyData& body) +{ + return body.m_invMass; +} + + +#endif//B3_RIGID_BODY_CL diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h new file mode 100644 index 000000000000..8788ccbb4711 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h @@ -0,0 +1,20 @@ + +#ifndef B3_BVH_SUBTREE_INFO_DATA_H +#define B3_BVH_SUBTREE_INFO_DATA_H + +typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t; + +struct b3BvhSubtreeInfoData +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes, points to the root of the subtree + int m_rootNodeIndex; + //4 bytes + int m_subtreeSize; + int m_padding[3]; +}; + +#endif //B3_BVH_SUBTREE_INFO_DATA_H + diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h new file mode 100644 index 000000000000..2618da24bca5 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h @@ -0,0 +1,126 @@ + + +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h" + + + +// work-in-progress +void b3BvhTraversal( __global const b3Int4* pairs, + __global const b3RigidBodyData* rigidBodies, + __global const b3Collidable* collidables, + __global b3Aabb* aabbs, + __global b3Int4* concavePairsOut, + __global volatile int* numConcavePairsOut, + __global const b3BvhSubtreeInfo* subtreeHeadersRoot, + __global const b3QuantizedBvhNode* quantizedNodesRoot, + __global const b3BvhInfo* bvhInfos, + int numPairs, + int maxNumConcavePairsCapacity, + int id) +{ + + int bodyIndexA = pairs[id].x; + int bodyIndexB = pairs[id].y; + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + //once the broadphase avoids static-static pairs, we can remove this test + if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0)) + { + return; + } + + if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH) + return; + + int shapeTypeB = collidables[collidableIndexB].m_shapeType; + + if (shapeTypeB!=SHAPE_CONVEX_HULL && + shapeTypeB!=SHAPE_SPHERE && + shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS + ) + return; + + b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes]; + + b3Float4 bvhAabbMin = bvhInfo.m_aabbMin; + b3Float4 bvhAabbMax = bvhInfo.m_aabbMax; + b3Float4 bvhQuantization = bvhInfo.m_quantization; + int numSubtreeHeaders = bvhInfo.m_numSubTrees; + __global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset]; + __global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset]; + + + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + b3QuantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_minVec,false,bvhAabbMin, bvhAabbMax,bvhQuantization); + b3QuantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_maxVec,true ,bvhAabbMin, bvhAabbMax,bvhQuantization); + + for (int i=0;i= 0, so output intersection + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + } + } + else + { + if (de<0) + { + // Start >= 0, end < 0 so output intersection and end + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + ppVtxOut[numVertsOut++] = endVertex; + } + } + firstVertex = endVertex; + ds = de; + } + return numVertsOut; +} + + +__kernel void clipFacesAndFindContactsKernel( __global const b3Float4* separatingNormals, + __global const int* hasSeparatingAxis, + __global b3Int4* clippingFacesOut, + __global b3Float4* worldVertsA1, + __global b3Float4* worldNormalsA1, + __global b3Float4* worldVertsB1, + __global b3Float4* worldVertsB2, + int vertexFaceCapacity, + int pairIndex + ) +{ +// int i = get_global_id(0); + //int pairIndex = i; + int i = pairIndex; + + float minDist = -1e30f; + float maxDist = 0.02f; + +// if (i=0) + { + + + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + + for(int e0=0;e0m_worldNormalOnB.w; +}; + +inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints) +{ + contact->m_worldNormalOnB.w = (float)numPoints; +}; + + + +#endif //B3_CONTACT4DATA_H \ No newline at end of file diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h new file mode 100644 index 000000000000..f295f01a6c3e --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h @@ -0,0 +1,520 @@ + +#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H +#define B3_CONTACT_CONVEX_CONVEX_SAT_H + + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h" + +#define B3_MAX_VERTS 1024 + + + +inline b3Float4 b3Lerp3(const b3Float4& a,const b3Float4& b, float t) +{ + return b3MakeVector3( a.x + (b.x - a.x) * t, + a.y + (b.y - a.y) * t, + a.z + (b.z - a.z) * t, + 0.f); +} + + +// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut +inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS,float planeEqWS, b3Float4* ppVtxOut) +{ + + int ve; + float ds, de; + int numVertsOut = 0; + if (numVertsIn < 2) + return 0; + + b3Float4 firstVertex=pVtxIn[numVertsIn-1]; + b3Float4 endVertex = pVtxIn[0]; + + ds = b3Dot3F4(planeNormalWS,firstVertex)+planeEqWS; + + for (ve = 0; ve < numVertsIn; ve++) + { + endVertex=pVtxIn[ve]; + + de = b3Dot3F4(planeNormalWS,endVertex)+planeEqWS; + + if (ds<0) + { + if (de<0) + { + // Start < 0, end < 0, so output endVertex + ppVtxOut[numVertsOut++] = endVertex; + } + else + { + // Start < 0, end >= 0, so output intersection + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + } + } + else + { + if (de<0) + { + // Start >= 0, end < 0 so output intersection and end + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + ppVtxOut[numVertsOut++] = endVertex; + } + } + firstVertex = endVertex; + ds = de; + } + return numVertsOut; +} + + +inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA, + const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1, + b3Float4* worldVertsB2, int capacityWorldVertsB2, + const float minDist, float maxDist, + const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& facesA, const b3AlignedObjectArray& indicesA, + //const b3Float4* verticesB, const b3GpuFace* facesB, const int* indicesB, + b3Float4* contactsOut, + int contactCapacity) +{ + int numContactsOut = 0; + + b3Float4* pVtxIn = worldVertsB1; + b3Float4* pVtxOut = worldVertsB2; + + int numVertsIn = numWorldVertsB1; + int numVertsOut = 0; + + int closestFaceA=-1; + { + float dmin = FLT_MAX; + for(int face=0;facem_numFaces;face++) + { + const b3Float4 Normal = b3MakeVector3( + facesA[hullA->m_faceOffset+face].m_plane.x, + facesA[hullA->m_faceOffset+face].m_plane.y, + facesA[hullA->m_faceOffset+face].m_plane.z,0.f); + const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal); + + float d = b3Dot3F4(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + } + } + } + if (closestFaceA<0) + return numContactsOut; + + b3GpuFace polyA = facesA[hullA->m_faceOffset+closestFaceA]; + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + //int numContacts = numWorldVertsB1; + int numVerticesA = polyA.m_numIndices; + for(int e0=0;e0m_vertexOffset+indicesA[polyA.m_indexOffset+e0]]; + const b3Float4 b = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+((e0+1)%numVerticesA)]]; + const b3Float4 edge0 = a - b; + const b3Float4 WorldEdge0 = b3QuatRotate(ornA,edge0); + b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA,planeNormalA); + + b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0,worldPlaneAnormal1); + b3Float4 worldA1 = b3TransformPoint(a,posA,ornA); + float planeEqWS1 = -b3Dot3F4(worldA1,planeNormalWS1); + + b3Float4 planeNormalWS = planeNormalWS1; + float planeEqWS=planeEqWS1; + + //clip face + //clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS); + numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut); + + //btSwap(pVtxIn,pVtxOut); + b3Float4* tmp = pVtxOut; + pVtxOut = pVtxIn; + pVtxIn = tmp; + numVertsIn = numVertsOut; + numVertsOut = 0; + } + + + // only keep points that are behind the witness face + { + b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + float localPlaneEq = polyA.m_plane.w; + b3Float4 planeNormalWS = b3QuatRotate(ornA,localPlaneNormal); + float planeEqWS=localPlaneEq-b3Dot3F4(planeNormalWS,posA); + for (int i=0;i& verticesA, const b3AlignedObjectArray& facesA, const b3AlignedObjectArray& indicesA, + const b3AlignedObjectArray& verticesB, const b3AlignedObjectArray& facesB, const b3AlignedObjectArray& indicesB, + + b3Float4* contactsOut, + int contactCapacity) +{ + int numContactsOut = 0; + int numWorldVertsB1= 0; + + B3_PROFILE("clipHullAgainstHull"); + + //float curMaxDist=maxDist; + int closestFaceB=-1; + float dmax = -FLT_MAX; + + { + //B3_PROFILE("closestFaceB"); + if (hullB.m_numFaces!=1) + { + //printf("wtf\n"); + } + static bool once = true; + //printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z); + + for(int face=0;facem_numIndices;i++) + { + b3Float4 vert = verticesB[hullB.m_vertexOffset+indicesB[faceB->m_indexOffset+i]]; + printf("vert[%d] = %f,%f,%f\n",i,vert.x,vert.y,vert.z); + } + } +#endif //BT_DEBUG_SAT_FACE + //if (facesB[hullB.m_faceOffset+face].m_numIndices>2) + { + const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset+face].m_plane.x, + facesB[hullB.m_faceOffset+face].m_plane.y, facesB[hullB.m_faceOffset+face].m_plane.z,0.f); + const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal); +#ifdef BT_DEBUG_SAT_FACE + if (once) + printf("faceNormal = %f,%f,%f\n",Normal.x,Normal.y,Normal.z); +#endif + float d = b3Dot3F4(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + once = false; + } + + + b3Assert(closestFaceB>=0); + { + //B3_PROFILE("worldVertsB1"); + const b3GpuFace& polyB = facesB[hullB.m_faceOffset+closestFaceB]; + const int numVertices = polyB.m_numIndices; + for(int e0=0;e0=0) + { + //B3_PROFILE("clipFaceAgainstHull"); + numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA, + posA,ornA, + worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist, + verticesA, facesA, indicesA, + contactsOut,contactCapacity); + } + + return numContactsOut; +} + + + + +inline int b3ClipHullHullSingle( + int bodyIndexA, int bodyIndexB, + const b3Float4& posA, + const b3Quaternion& ornA, + const b3Float4& posB, + const b3Quaternion& ornB, + + int collidableIndexA, int collidableIndexB, + + const b3AlignedObjectArray* bodyBuf, + b3AlignedObjectArray* globalContactOut, + int& nContacts, + + const b3AlignedObjectArray& hostConvexDataA, + const b3AlignedObjectArray& hostConvexDataB, + + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& uniqueEdgesA, + const b3AlignedObjectArray& facesA, + const b3AlignedObjectArray& indicesA, + + const b3AlignedObjectArray& verticesB, + const b3AlignedObjectArray& uniqueEdgesB, + const b3AlignedObjectArray& facesB, + const b3AlignedObjectArray& indicesB, + + const b3AlignedObjectArray& hostCollidablesA, + const b3AlignedObjectArray& hostCollidablesB, + const b3Vector3& sepNormalWorldSpace, + int maxContactCapacity ) +{ + int contactIndex = -1; + b3ConvexPolyhedronData hullA, hullB; + + b3Collidable colA = hostCollidablesA[collidableIndexA]; + hullA = hostConvexDataA[colA.m_shapeIndex]; + //printf("numvertsA = %d\n",hullA.m_numVertices); + + + b3Collidable colB = hostCollidablesB[collidableIndexB]; + hullB = hostConvexDataB[colB.m_shapeIndex]; + //printf("numvertsB = %d\n",hullB.m_numVertices); + + + b3Float4 contactsOut[B3_MAX_VERTS]; + int localContactCapacity = B3_MAX_VERTS; + +#ifdef _WIN32 + b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x)); + b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x)); +#endif + + + { + + b3Float4 worldVertsB1[B3_MAX_VERTS]; + b3Float4 worldVertsB2[B3_MAX_VERTS]; + int capacityWorldVerts = B3_MAX_VERTS; + + b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z,0.f); + int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex; + int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex; + + b3Scalar minDist = -1; + b3Scalar maxDist = 0.; + + + + b3Transform trA,trB; + { + //B3_PROFILE("b3TransformPoint computation"); + //trA.setIdentity(); + trA.setOrigin(b3MakeVector3(posA.x,posA.y,posA.z)); + trA.setRotation(b3Quaternion(ornA.x,ornA.y,ornA.z,ornA.w)); + + //trB.setIdentity(); + trB.setOrigin(b3MakeVector3(posB.x,posB.y,posB.z)); + trB.setRotation(b3Quaternion(ornB.x,ornB.y,ornB.z,ornB.w)); + } + + b3Quaternion trAorn = trA.getRotation(); + b3Quaternion trBorn = trB.getRotation(); + + int numContactsOut = b3ClipHullAgainstHull(hostNormal, + hostConvexDataA.at(shapeA), + hostConvexDataB.at(shapeB), + (b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn, + (b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn, + worldVertsB1,worldVertsB2,capacityWorldVerts, + minDist, maxDist, + verticesA, facesA,indicesA, + verticesB, facesB,indicesB, + + contactsOut,localContactCapacity); + + if (numContactsOut>0) + { + B3_PROFILE("overlap"); + + b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal; +// b3Float4 centerOut; + + b3Int4 contactIdx; + contactIdx.x = 0; + contactIdx.y = 1; + contactIdx.z = 2; + contactIdx.w = 3; + + int numPoints = 0; + + { + B3_PROFILE("extractManifold"); + numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx); + } + + b3Assert(numPoints); + + if (nContactsexpand(); + b3Contact4Data& contact = globalContactOut->at(nContacts); + contact.m_batchIdx = 0;//i; + contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA; + contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB; + + contact.m_frictionCoeffCmp = 45874; + contact.m_restituitionCoeffCmp = 0; + + // float distance = 0.f; + for (int p=0;p& rigidBodies, + const b3AlignedObjectArray& collidables, + const b3AlignedObjectArray& convexShapes, + const b3AlignedObjectArray& convexVertices, + const b3AlignedObjectArray& uniqueEdges, + const b3AlignedObjectArray& convexIndices, + const b3AlignedObjectArray& faces, + b3AlignedObjectArray& globalContactsOut, + int& nGlobalContactsOut, + int maxContactCapacity) +{ + int contactIndex = -1; + + + b3Float4 posA = rigidBodies[bodyIndexA].m_pos; + b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat; + b3Float4 posB = rigidBodies[bodyIndexB].m_pos; + b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat; + + + b3ConvexPolyhedronData hullA, hullB; + + b3Float4 sepNormalWorldSpace; + + + + b3Collidable colA = collidables[collidableIndexA]; + hullA = convexShapes[colA.m_shapeIndex]; + //printf("numvertsA = %d\n",hullA.m_numVertices); + + + b3Collidable colB = collidables[collidableIndexB]; + hullB = convexShapes[colB.m_shapeIndex]; + //printf("numvertsB = %d\n",hullB.m_numVertices); + + + + +#ifdef _WIN32 + b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x)); + b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x)); +#endif + + bool foundSepAxis = b3FindSeparatingAxis(hullA,hullB, + posA, + ornA, + posB, + ornB, + + convexVertices,uniqueEdges,faces,convexIndices, + convexVertices,uniqueEdges,faces,convexIndices, + + sepNormalWorldSpace + ); + + + if (foundSepAxis) + { + + + contactIndex = b3ClipHullHullSingle( + bodyIndexA, bodyIndexB, + posA,ornA, + posB,ornB, + collidableIndexA, collidableIndexB, + &rigidBodies, + &globalContactsOut, + nGlobalContactsOut, + + convexShapes, + convexShapes, + + convexVertices, + uniqueEdges, + faces, + convexIndices, + + convexVertices, + uniqueEdges, + faces, + convexIndices, + + collidables, + collidables, + sepNormalWorldSpace, + maxContactCapacity); + + } + + return contactIndex; +} + +#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h new file mode 100644 index 000000000000..a3fa82287b82 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h @@ -0,0 +1,162 @@ + +#ifndef B3_CONTACT_SPHERE_SPHERE_H +#define B3_CONTACT_SPHERE_SPHERE_H + + + + + +void computeContactSphereConvex(int pairIndex, + int bodyIndexA, int bodyIndexB, + int collidableIndexA, int collidableIndexB, + const b3RigidBodyData* rigidBodies, + const b3Collidable* collidables, + const b3ConvexPolyhedronData* convexShapes, + const b3Vector3* convexVertices, + const int* convexIndices, + const b3GpuFace* faces, + b3Contact4* globalContactsOut, + int& nGlobalContactsOut, + int maxContactCapacity) +{ + + float radius = collidables[collidableIndexA].m_radius; + float4 spherePos1 = rigidBodies[bodyIndexA].m_pos; + b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat; + + + + float4 pos = rigidBodies[bodyIndexB].m_pos; + + + b3Quaternion quat = rigidBodies[bodyIndexB].m_quat; + + b3Transform tr; + tr.setIdentity(); + tr.setOrigin(pos); + tr.setRotation(quat); + b3Transform trInv = tr.inverse(); + + float4 spherePos = trInv(spherePos1); + + int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx; + int shapeIndex = collidables[collidableIndex].m_shapeIndex; + int numFaces = convexShapes[shapeIndex].m_numFaces; + float4 closestPnt = b3MakeVector3(0, 0, 0, 0); + float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0); + float minDist = -1000000.f; // TODO: What is the largest/smallest float? + bool bCollide = true; + int region = -1; + float4 localHitNormal; + for ( int f = 0; f < numFaces; f++ ) + { + b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f]; + float4 planeEqn; + float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); + float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal); + planeEqn = n1; + planeEqn[3] = face.m_plane.w; + + float4 pntReturn; + float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn); + + if ( dist > radius) + { + bCollide = false; + break; + } + + if ( dist > 0 ) + { + //might hit an edge or vertex + b3Vector3 out; + + bool isInPoly = IsPointInPolygon(spherePos, + &face, + &convexVertices[convexShapes[shapeIndex].m_vertexOffset], + convexIndices, + &out); + if (isInPoly) + { + if (dist>minDist) + { + minDist = dist; + closestPnt = pntReturn; + localHitNormal = planeEqn; + region=1; + } + } else + { + b3Vector3 tmp = spherePos-out; + b3Scalar l2 = tmp.length2(); + if (l2minDist) + { + minDist = dist; + closestPnt = out; + localHitNormal = tmp/dist; + region=2; + } + + } else + { + bCollide = false; + break; + } + } + } + else + { + if ( dist > minDist ) + { + minDist = dist; + closestPnt = pntReturn; + localHitNormal = planeEqn; + region=3; + } + } + } + static int numChecks = 0; + numChecks++; + + if (bCollide && minDist > -10000) + { + + float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld; + float4 pOnB1 = tr(closestPnt); + //printf("dist ,%f,",minDist); + float actualDepth = minDist-radius; + if (actualDepth<0) + { + //printf("actualDepth = ,%f,", actualDepth); + //printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z); + //printf("region=,%d,\n", region); + pOnB1[3] = actualDepth; + + int dstIdx; +// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx ); + + if (nGlobalContactsOut < maxContactCapacity) + { + dstIdx=nGlobalContactsOut; + nGlobalContactsOut++; + + b3Contact4* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = normalOnSurfaceB1; + c->setFrictionCoeff(0.7); + c->setRestituitionCoeff(0.f); + + c->m_batchIdx = pairIndex; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB; + c->m_worldPosB[0] = pOnB1; + int numPoints = 1; + c->m_worldNormalOnB.w = (b3Scalar)numPoints; + }//if (dstIdx < numPairs) + } + }//if (hasCollision) + +} +#endif //B3_CONTACT_SPHERE_SPHERE_H diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h new file mode 100644 index 000000000000..5c5f4e297fcf --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h @@ -0,0 +1,40 @@ + +#ifndef B3_CONVEX_POLYHEDRON_DATA_H +#define B3_CONVEX_POLYHEDRON_DATA_H + + + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Common/shared/b3Quat.h" + +typedef struct b3GpuFace b3GpuFace_t; +struct b3GpuFace +{ + b3Float4 m_plane; + int m_indexOffset; + int m_numIndices; + int m_unusedPadding1; + int m_unusedPadding2; +}; + +typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t; + +struct b3ConvexPolyhedronData +{ + b3Float4 m_localCenter; + b3Float4 m_extents; + b3Float4 mC; + b3Float4 mE; + + float m_radius; + int m_faceOffset; + int m_numFaces; + int m_numVertices; + + int m_vertexOffset; + int m_uniqueEdgesOffset; + int m_numUniqueEdges; + int m_unused; +}; + +#endif //B3_CONVEX_POLYHEDRON_DATA_H diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h new file mode 100644 index 000000000000..89993f35654a --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h @@ -0,0 +1,832 @@ +#ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H +#define B3_FIND_CONCAVE_SEPARATING_AXIS_H + +#define B3_TRIANGLE_NUM_CONVEX_FACES 5 + + +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" + + +inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn, +const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max) +{ + min[0] = FLT_MAX; + max[0] = -FLT_MAX; + int numVerts = hull->m_numVertices; + + const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),*dir); + float offset = b3Dot(pos,*dir); + for(int i=0;im_vertexOffset+i],localDir); + if(dp < min[0]) + min[0] = dp; + if(dp > max[0]) + max[0] = dp; + } + if(min[0]>max[0]) + { + float tmp = min[0]; + min[0] = max[0]; + max[0] = tmp; + } + min[0] += offset; + max[0] += offset; +} + + +inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, + b3Float4ConstArg posA,b3QuatConstArg ornA, + b3Float4ConstArg posB,b3QuatConstArg ornB, + b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB,float* depth) +{ + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1); + + if(Max0m_numFaces*hullB->m_numVertices; + curFaceVertexAB+= hullB->m_numFaces*hullA->m_numVertices; + + if (curFaceVertexAB>maxFaceVertex) + { + maxFaceVertex = curFaceVertexAB; + printf("curFaceVertexAB = %d\n",curFaceVertexAB); + printf("hullA->m_numFaces = %d\n",hullA->m_numFaces); + printf("hullA->m_numVertices = %d\n",hullA->m_numVertices); + printf("hullB->m_numVertices = %d\n",hullB->m_numVertices); + } +*/ + + int curPlaneTests=0; + { + int numFacesA = hullA->m_numFaces; + // Test normals from hullA + for(int i=0;im_faceOffset+i].m_plane; + b3Float4 faceANormalWS = b3QuatRotate(ornA,normal); + if (b3Dot(DeltaC2,faceANormalWS)<0) + faceANormalWS*=-1.f; + curPlaneTests++; + float d; + if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d)) + return false; + if(d<*dmin) + { + *dmin = d; + *sep = faceANormalWS; + } + } + } + if((b3Dot(-DeltaC2,*sep))>0.0f) + { + *sep = -(*sep); + } + return true; +} + + +b3Vector3 unitSphere162[]= +{ + b3MakeVector3(0.000000,-1.000000,0.000000), +b3MakeVector3(0.203181,-0.967950,0.147618), +b3MakeVector3(-0.077607,-0.967950,0.238853), +b3MakeVector3(0.723607,-0.447220,0.525725), +b3MakeVector3(0.609547,-0.657519,0.442856), +b3MakeVector3(0.812729,-0.502301,0.295238), +b3MakeVector3(-0.251147,-0.967949,0.000000), +b3MakeVector3(-0.077607,-0.967950,-0.238853), +b3MakeVector3(0.203181,-0.967950,-0.147618), +b3MakeVector3(0.860698,-0.251151,0.442858), +b3MakeVector3(-0.276388,-0.447220,0.850649), +b3MakeVector3(-0.029639,-0.502302,0.864184), +b3MakeVector3(-0.155215,-0.251152,0.955422), +b3MakeVector3(-0.894426,-0.447216,0.000000), +b3MakeVector3(-0.831051,-0.502299,0.238853), +b3MakeVector3(-0.956626,-0.251149,0.147618), +b3MakeVector3(-0.276388,-0.447220,-0.850649), +b3MakeVector3(-0.483971,-0.502302,-0.716565), +b3MakeVector3(-0.436007,-0.251152,-0.864188), +b3MakeVector3(0.723607,-0.447220,-0.525725), +b3MakeVector3(0.531941,-0.502302,-0.681712), +b3MakeVector3(0.687159,-0.251152,-0.681715), +b3MakeVector3(0.687159,-0.251152,0.681715), +b3MakeVector3(-0.436007,-0.251152,0.864188), +b3MakeVector3(-0.956626,-0.251149,-0.147618), +b3MakeVector3(-0.155215,-0.251152,-0.955422), +b3MakeVector3(0.860698,-0.251151,-0.442858), +b3MakeVector3(0.276388,0.447220,0.850649), +b3MakeVector3(0.483971,0.502302,0.716565), +b3MakeVector3(0.232822,0.657519,0.716563), +b3MakeVector3(-0.723607,0.447220,0.525725), +b3MakeVector3(-0.531941,0.502302,0.681712), +b3MakeVector3(-0.609547,0.657519,0.442856), +b3MakeVector3(-0.723607,0.447220,-0.525725), +b3MakeVector3(-0.812729,0.502301,-0.295238), +b3MakeVector3(-0.609547,0.657519,-0.442856), +b3MakeVector3(0.276388,0.447220,-0.850649), +b3MakeVector3(0.029639,0.502302,-0.864184), +b3MakeVector3(0.232822,0.657519,-0.716563), +b3MakeVector3(0.894426,0.447216,0.000000), +b3MakeVector3(0.831051,0.502299,-0.238853), +b3MakeVector3(0.753442,0.657515,0.000000), +b3MakeVector3(-0.232822,-0.657519,0.716563), +b3MakeVector3(-0.162456,-0.850654,0.499995), +b3MakeVector3(0.052790,-0.723612,0.688185), +b3MakeVector3(0.138199,-0.894429,0.425321), +b3MakeVector3(0.262869,-0.525738,0.809012), +b3MakeVector3(0.361805,-0.723611,0.587779), +b3MakeVector3(0.531941,-0.502302,0.681712), +b3MakeVector3(0.425323,-0.850654,0.309011), +b3MakeVector3(0.812729,-0.502301,-0.295238), +b3MakeVector3(0.609547,-0.657519,-0.442856), +b3MakeVector3(0.850648,-0.525736,0.000000), +b3MakeVector3(0.670817,-0.723611,-0.162457), +b3MakeVector3(0.670817,-0.723610,0.162458), +b3MakeVector3(0.425323,-0.850654,-0.309011), +b3MakeVector3(0.447211,-0.894428,0.000001), +b3MakeVector3(-0.753442,-0.657515,0.000000), +b3MakeVector3(-0.525730,-0.850652,0.000000), +b3MakeVector3(-0.638195,-0.723609,0.262864), +b3MakeVector3(-0.361801,-0.894428,0.262864), +b3MakeVector3(-0.688189,-0.525736,0.499997), +b3MakeVector3(-0.447211,-0.723610,0.525729), +b3MakeVector3(-0.483971,-0.502302,0.716565), +b3MakeVector3(-0.232822,-0.657519,-0.716563), +b3MakeVector3(-0.162456,-0.850654,-0.499995), +b3MakeVector3(-0.447211,-0.723611,-0.525727), +b3MakeVector3(-0.361801,-0.894429,-0.262863), +b3MakeVector3(-0.688189,-0.525736,-0.499997), +b3MakeVector3(-0.638195,-0.723609,-0.262863), +b3MakeVector3(-0.831051,-0.502299,-0.238853), +b3MakeVector3(0.361804,-0.723612,-0.587779), +b3MakeVector3(0.138197,-0.894429,-0.425321), +b3MakeVector3(0.262869,-0.525738,-0.809012), +b3MakeVector3(0.052789,-0.723611,-0.688186), +b3MakeVector3(-0.029639,-0.502302,-0.864184), +b3MakeVector3(0.956626,0.251149,0.147618), +b3MakeVector3(0.956626,0.251149,-0.147618), +b3MakeVector3(0.951058,-0.000000,0.309013), +b3MakeVector3(1.000000,0.000000,0.000000), +b3MakeVector3(0.947213,-0.276396,0.162458), +b3MakeVector3(0.951058,0.000000,-0.309013), +b3MakeVector3(0.947213,-0.276396,-0.162458), +b3MakeVector3(0.155215,0.251152,0.955422), +b3MakeVector3(0.436007,0.251152,0.864188), +b3MakeVector3(-0.000000,-0.000000,1.000000), +b3MakeVector3(0.309017,0.000000,0.951056), +b3MakeVector3(0.138199,-0.276398,0.951055), +b3MakeVector3(0.587786,0.000000,0.809017), +b3MakeVector3(0.447216,-0.276398,0.850648), +b3MakeVector3(-0.860698,0.251151,0.442858), +b3MakeVector3(-0.687159,0.251152,0.681715), +b3MakeVector3(-0.951058,-0.000000,0.309013), +b3MakeVector3(-0.809018,0.000000,0.587783), +b3MakeVector3(-0.861803,-0.276396,0.425324), +b3MakeVector3(-0.587786,0.000000,0.809017), +b3MakeVector3(-0.670819,-0.276397,0.688191), +b3MakeVector3(-0.687159,0.251152,-0.681715), +b3MakeVector3(-0.860698,0.251151,-0.442858), +b3MakeVector3(-0.587786,-0.000000,-0.809017), +b3MakeVector3(-0.809018,-0.000000,-0.587783), +b3MakeVector3(-0.670819,-0.276397,-0.688191), +b3MakeVector3(-0.951058,0.000000,-0.309013), +b3MakeVector3(-0.861803,-0.276396,-0.425324), +b3MakeVector3(0.436007,0.251152,-0.864188), +b3MakeVector3(0.155215,0.251152,-0.955422), +b3MakeVector3(0.587786,-0.000000,-0.809017), +b3MakeVector3(0.309017,-0.000000,-0.951056), +b3MakeVector3(0.447216,-0.276398,-0.850648), +b3MakeVector3(0.000000,0.000000,-1.000000), +b3MakeVector3(0.138199,-0.276398,-0.951055), +b3MakeVector3(0.670820,0.276396,0.688190), +b3MakeVector3(0.809019,-0.000002,0.587783), +b3MakeVector3(0.688189,0.525736,0.499997), +b3MakeVector3(0.861804,0.276394,0.425323), +b3MakeVector3(0.831051,0.502299,0.238853), +b3MakeVector3(-0.447216,0.276397,0.850649), +b3MakeVector3(-0.309017,-0.000001,0.951056), +b3MakeVector3(-0.262869,0.525738,0.809012), +b3MakeVector3(-0.138199,0.276397,0.951055), +b3MakeVector3(0.029639,0.502302,0.864184), +b3MakeVector3(-0.947213,0.276396,-0.162458), +b3MakeVector3(-1.000000,0.000001,0.000000), +b3MakeVector3(-0.850648,0.525736,-0.000000), +b3MakeVector3(-0.947213,0.276397,0.162458), +b3MakeVector3(-0.812729,0.502301,0.295238), +b3MakeVector3(-0.138199,0.276397,-0.951055), +b3MakeVector3(-0.309016,-0.000000,-0.951057), +b3MakeVector3(-0.262869,0.525738,-0.809012), +b3MakeVector3(-0.447215,0.276397,-0.850649), +b3MakeVector3(-0.531941,0.502302,-0.681712), +b3MakeVector3(0.861804,0.276396,-0.425322), +b3MakeVector3(0.809019,0.000000,-0.587782), +b3MakeVector3(0.688189,0.525736,-0.499997), +b3MakeVector3(0.670821,0.276397,-0.688189), +b3MakeVector3(0.483971,0.502302,-0.716565), +b3MakeVector3(0.077607,0.967950,0.238853), +b3MakeVector3(0.251147,0.967949,0.000000), +b3MakeVector3(0.000000,1.000000,0.000000), +b3MakeVector3(0.162456,0.850654,0.499995), +b3MakeVector3(0.361800,0.894429,0.262863), +b3MakeVector3(0.447209,0.723612,0.525728), +b3MakeVector3(0.525730,0.850652,0.000000), +b3MakeVector3(0.638194,0.723610,0.262864), +b3MakeVector3(-0.203181,0.967950,0.147618), +b3MakeVector3(-0.425323,0.850654,0.309011), +b3MakeVector3(-0.138197,0.894430,0.425320), +b3MakeVector3(-0.361804,0.723612,0.587778), +b3MakeVector3(-0.052790,0.723612,0.688185), +b3MakeVector3(-0.203181,0.967950,-0.147618), +b3MakeVector3(-0.425323,0.850654,-0.309011), +b3MakeVector3(-0.447210,0.894429,0.000000), +b3MakeVector3(-0.670817,0.723611,-0.162457), +b3MakeVector3(-0.670817,0.723611,0.162457), +b3MakeVector3(0.077607,0.967950,-0.238853), +b3MakeVector3(0.162456,0.850654,-0.499995), +b3MakeVector3(-0.138197,0.894430,-0.425320), +b3MakeVector3(-0.052790,0.723612,-0.688185), +b3MakeVector3(-0.361804,0.723612,-0.587778), +b3MakeVector3(0.361800,0.894429,-0.262863), +b3MakeVector3(0.638194,0.723610,-0.262864), +b3MakeVector3(0.447209,0.723612,-0.525728) +}; + + +bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, + b3Float4ConstArg posA1, + b3QuatConstArg ornA, + b3Float4ConstArg posB1, + b3QuatConstArg ornB, + b3Float4ConstArg DeltaC2, + const b3Float4* verticesA, + const b3Float4* uniqueEdgesA, + const b3GpuFace* facesA, + const int* indicesA, + __global const b3Float4* verticesB, + __global const b3Float4* uniqueEdgesB, + __global const b3GpuFace* facesB, + __global const int* indicesB, + b3Float4* sep, + float* dmin, + bool searchAllEdgeEdge) +{ + + + b3Float4 posA = posA1; + posA.w = 0.f; + b3Float4 posB = posB1; + posB.w = 0.f; + +// int curPlaneTests=0; + + int curEdgeEdge = 0; + // Test edges + static int maxEdgeTests = 0; + int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges; + if (curEdgeTests >maxEdgeTests ) + { + maxEdgeTests = curEdgeTests ; + printf("maxEdgeTests = %d\n",maxEdgeTests ); + printf("hullA->m_numUniqueEdges = %d\n",hullA->m_numUniqueEdges); + printf("hullB->m_numUniqueEdges = %d\n",hullB->m_numUniqueEdges); + + } + + + if (searchAllEdgeEdge) + { + for(int e0=0;e0m_numUniqueEdges;e0++) + { + const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0]; + b3Float4 edge0World = b3QuatRotate(ornA,edge0); + + for(int e1=0;e1m_numUniqueEdges;e1++) + { + const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1]; + b3Float4 edge1World = b3QuatRotate(ornB,edge1); + + + b3Float4 crossje = b3Cross(edge0World,edge1World); + + curEdgeEdge++; + if(!b3IsAlmostZero(crossje)) + { + crossje = b3Normalized(crossje); + if (b3Dot(DeltaC2,crossje)<0) + crossje *= -1.f; + + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); + + if(Max00) + { + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); + + if(Max00.0f) + { + *sep = -(*sep); + } + return true; +} + + + +inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal, + __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB, + b3Float4ConstArg posA, b3QuatConstArg ornA,b3Float4ConstArg posB, b3QuatConstArg ornB, + __global b3Float4* worldVertsA1, + __global b3Float4* worldNormalsA1, + __global b3Float4* worldVertsB1, + int capacityWorldVerts, + const float minDist, float maxDist, + __global const b3Float4* verticesA, + __global const b3GpuFace_t* facesA, + __global const int* indicesA, + __global const b3Float4* verticesB, + __global const b3GpuFace_t* facesB, + __global const int* indicesB, + + __global b3Int4* clippingFaces, int pairIndex) +{ + int numContactsOut = 0; + int numWorldVertsB1= 0; + + + int closestFaceB=-1; + float dmax = -FLT_MAX; + + { + for(int face=0;facem_numFaces;face++) + { + const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset+face].m_plane.x, + facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f); + const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal); + float d = b3Dot(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + + { + const b3GpuFace_t polyB = facesB[hullB->m_faceOffset+closestFaceB]; + const int numVertices = polyB.m_numIndices; + for(int e0=0;e0m_vertexOffset+indicesB[polyB.m_indexOffset+e0]]; + worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = b3TransformPoint(b,posB,ornB); + } + } + + int closestFaceA=-1; + { + float dmin = FLT_MAX; + for(int face=0;facem_numFaces;face++) + { + const b3Float4 Normal = b3MakeFloat4( + facesA[hullA->m_faceOffset+face].m_plane.x, + facesA[hullA->m_faceOffset+face].m_plane.y, + facesA[hullA->m_faceOffset+face].m_plane.z, + 0.f); + const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal); + + float d = b3Dot(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + worldNormalsA1[pairIndex] = faceANormalWS; + } + } + } + + int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices; + for(int e0=0;e0m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]]; + worldVertsA1[pairIndex*capacityWorldVerts+e0] = b3TransformPoint(a, posA,ornA); + } + + clippingFaces[pairIndex].x = closestFaceA; + clippingFaces[pairIndex].y = closestFaceB; + clippingFaces[pairIndex].z = numVerticesA; + clippingFaces[pairIndex].w = numWorldVertsB1; + + + return numContactsOut; +} + + + + +__kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs, + __global const b3RigidBodyData* rigidBodies, + __global const b3Collidable* collidables, + __global const b3ConvexPolyhedronData* convexShapes, + __global const b3Float4* vertices, + __global const b3Float4* uniqueEdges, + __global const b3GpuFace* faces, + __global const int* indices, + __global const b3GpuChildShape* gpuChildShapes, + __global b3Aabb* aabbs, + __global b3Float4* concaveSeparatingNormalsOut, + __global b3Int4* clippingFacesOut, + __global b3Vector3* worldVertsA1Out, + __global b3Vector3* worldNormalsA1Out, + __global b3Vector3* worldVertsB1Out, + __global int* hasSeparatingNormals, + int vertexFaceCapacity, + int numConcavePairs, + int pairIdx + ) +{ + int i = pairIdx; +/* int i = get_global_id(0); + if (i>=numConcavePairs) + return; + int pairIdx = i; + */ + + int bodyIndexA = concavePairs[i].x; + int bodyIndexB = concavePairs[i].y; + + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&& + collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + concavePairs[pairIdx].w = -1; + return; + } + + hasSeparatingNormals[i] = 0; + +// int numFacesA = convexShapes[shapeIndexA].m_numFaces; + int numActualConcaveConvexTests = 0; + + int f = concavePairs[i].z; + + bool overlap = false; + + b3ConvexPolyhedronData convexPolyhedronA; + + //add 3 vertices of the triangle + convexPolyhedronA.m_numVertices = 3; + convexPolyhedronA.m_vertexOffset = 0; + b3Float4 localCenter = b3MakeFloat4(0.f,0.f,0.f,0.f); + + b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f]; + b3Aabb triAabb; + triAabb.m_minVec = b3MakeFloat4(1e30f,1e30f,1e30f,0.f); + triAabb.m_maxVec = b3MakeFloat4(-1e30f,-1e30f,-1e30f,0.f); + + b3Float4 verticesA[3]; + for (int i=0;i<3;i++) + { + int index = indices[face.m_indexOffset+i]; + b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index]; + verticesA[i] = vert; + localCenter += vert; + + triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec,vert); + triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec,vert); + + } + + overlap = true; + overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap; + overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap; + overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap; + + if (overlap) + { + float dmin = FLT_MAX; + int hasSeparatingAxis=5; + b3Float4 sepAxis=b3MakeFloat4(1,2,3,4); + + // int localCC=0; + numActualConcaveConvexTests++; + + //a triangle has 3 unique edges + convexPolyhedronA.m_numUniqueEdges = 3; + convexPolyhedronA.m_uniqueEdgesOffset = 0; + b3Float4 uniqueEdgesA[3]; + + uniqueEdgesA[0] = (verticesA[1]-verticesA[0]); + uniqueEdgesA[1] = (verticesA[2]-verticesA[1]); + uniqueEdgesA[2] = (verticesA[0]-verticesA[2]); + + + convexPolyhedronA.m_faceOffset = 0; + + b3Float4 normal = b3MakeFloat4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); + + b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES]; + int indicesA[3+3+2+2+2]; + int curUsedIndices=0; + int fidx=0; + + //front size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[0] = 0; + indicesA[1] = 1; + indicesA[2] = 2; + curUsedIndices+=3; + float c = face.m_plane.w; + facesA[fidx].m_plane.x = normal.x; + facesA[fidx].m_plane.y = normal.y; + facesA[fidx].m_plane.z = normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + //back size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[3]=2; + indicesA[4]=1; + indicesA[5]=0; + curUsedIndices+=3; + float c = b3Dot(normal,verticesA[0]); + // float c1 = -face.m_plane.w; + facesA[fidx].m_plane.x = -normal.x; + facesA[fidx].m_plane.y = -normal.y; + facesA[fidx].m_plane.z = -normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + + bool addEdgePlanes = true; + if (addEdgePlanes) + { + int numVertices=3; + int prevVertex = numVertices-1; + for (int i=0;i& vertices, b3Scalar& min, b3Scalar& max) +{ + min = FLT_MAX; + max = -FLT_MAX; + int numVerts = hull.m_numVertices; + + const b3Float4 localDir = b3QuatRotate(orn.inverse(),dir); + + b3Scalar offset = b3Dot3F4(pos,dir); + + for(int i=0;i max) max = dp; + } + if(min>max) + { + b3Scalar tmp = min; + min = max; + max = tmp; + } + min += offset; + max += offset; +} + + +inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const b3Float4& posA,const b3Quaternion& ornA, + const b3Float4& posB,const b3Quaternion& ornB, + const b3Float4& sep_axis, const b3AlignedObjectArray& verticesA,const b3AlignedObjectArray& verticesB,b3Scalar& depth) +{ + b3Scalar Min0,Max0; + b3Scalar Min1,Max1; + b3ProjectAxis(hullA,posA,ornA,sep_axis,verticesA, Min0, Max0); + b3ProjectAxis(hullB,posB,ornB, sep_axis,verticesB, Min1, Max1); + + if(Max0=0.0f); + b3Scalar d1 = Max1 - Min0; + b3Assert(d1>=0.0f); + depth = d0& verticesA, + const b3AlignedObjectArray& uniqueEdgesA, + const b3AlignedObjectArray& facesA, + const b3AlignedObjectArray& indicesA, + const b3AlignedObjectArray& verticesB, + const b3AlignedObjectArray& uniqueEdgesB, + const b3AlignedObjectArray& facesB, + const b3AlignedObjectArray& indicesB, + + b3Vector3& sep) +{ + B3_PROFILE("findSeparatingAxis"); + + b3Float4 posA = posA1; + posA.w = 0.f; + b3Float4 posB = posB1; + posB.w = 0.f; +//#ifdef TEST_INTERNAL_OBJECTS + b3Float4 c0local = (b3Float4&)hullA.m_localCenter; + + b3Float4 c0 = b3TransformPoint(c0local, posA, ornA); + b3Float4 c1local = (b3Float4&)hullB.m_localCenter; + b3Float4 c1 = b3TransformPoint(c1local,posB,ornB); + const b3Float4 deltaC2 = c0 - c1; +//#endif + + b3Scalar dmin = FLT_MAX; + int curPlaneTests=0; + + int numFacesA = hullA.m_numFaces; + // Test normals from hullA + for(int i=0;i0.0f) + sep = -sep; + + return true; +} + +#endif //B3_FIND_SEPARATING_AXIS_H + diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h new file mode 100644 index 000000000000..6c3ad7c9dd9b --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h @@ -0,0 +1,920 @@ + +/*** + * --------------------------------- + * Copyright (c)2012 Daniel Fiser + * + * This file was ported from mpr.c file, part of libccd. + * The Minkoski Portal Refinement implementation was ported + * to OpenCL by Erwin Coumans for the Bullet 3 Physics library. + * at http://github.com/erwincoumans/bullet3 + * + * Distributed under the OSI-approved BSD License (the "License"); + * see . + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + + + + +#ifndef B3_MPR_PENETRATION_H +#define B3_MPR_PENETRATION_H + +#include "Bullet3Common/shared/b3PlatformDefinitions.h" +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" + + + + +#ifdef __cplusplus +#define B3_MPR_SQRT sqrtf +#else +#define B3_MPR_SQRT sqrt +#endif +#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y)) +#define B3_MPR_FABS fabs + +#define B3_MPR_TOLERANCE 1E-6f +#define B3_MPR_MAX_ITERATIONS 1000 + +struct _b3MprSupport_t +{ + b3Float4 v; //!< Support point in minkowski sum + b3Float4 v1; //!< Support point in obj1 + b3Float4 v2; //!< Support point in obj2 +}; +typedef struct _b3MprSupport_t b3MprSupport_t; + +struct _b3MprSimplex_t +{ + b3MprSupport_t ps[4]; + int last; //!< index of last added point +}; +typedef struct _b3MprSimplex_t b3MprSimplex_t; + +inline b3MprSupport_t* b3MprSimplexPointW(b3MprSimplex_t *s, int idx) +{ + return &s->ps[idx]; +} + +inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size) +{ + s->last = size - 1; +} + + +inline int b3MprSimplexSize(const b3MprSimplex_t *s) +{ + return s->last + 1; +} + + +inline const b3MprSupport_t* b3MprSimplexPoint(const b3MprSimplex_t* s, int idx) +{ + // here is no check on boundaries + return &s->ps[idx]; +} + +inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s) +{ + *d = *s; +} + +inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a) +{ + b3MprSupportCopy(s->ps + pos, a); +} + + +inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2) +{ + b3MprSupport_t supp; + + b3MprSupportCopy(&supp, &s->ps[pos1]); + b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]); + b3MprSupportCopy(&s->ps[pos2], &supp); +} + + +inline int b3MprIsZero(float val) +{ + return B3_MPR_FABS(val) < FLT_EPSILON; +} + + + +inline int b3MprEq(float _a, float _b) +{ + float ab; + float a, b; + + ab = B3_MPR_FABS(_a - _b); + if (B3_MPR_FABS(ab) < FLT_EPSILON) + return 1; + + a = B3_MPR_FABS(_a); + b = B3_MPR_FABS(_b); + if (b > a){ + return ab < FLT_EPSILON * b; + }else{ + return ab < FLT_EPSILON * a; + } +} + + +inline int b3MprVec3Eq(const b3Float4* a, const b3Float4 *b) +{ + return b3MprEq((*a).x, (*b).x) + && b3MprEq((*a).y, (*b).y) + && b3MprEq((*a).z, (*b).z); +} + + + +inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global const b3ConvexPolyhedronData_t* hull, b3ConstArray(b3Float4) verticesA) +{ + b3Float4 supVec = b3MakeFloat4(0,0,0,0); + float maxDot = -B3_LARGE_FLOAT; + + if( 0 < hull->m_numVertices ) + { + const b3Float4 scaled = supportVec; + int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot); + return verticesA[hull->m_vertexOffset+index]; + } + + return supVec; + +} + + +B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + const b3Float4* _dir, b3Float4* outp, int logme) +{ + //dir is in worldspace, move to local space + + b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos; + b3Quat orn = cpuBodyBuf[bodyIndex].m_quat; + + b3Float4 dir = b3MakeFloat4((*_dir).x,(*_dir).y,(*_dir).z,0.f); + + const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),dir); + + + //find local support vertex + int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx; + + b3Assert(cpuCollidables[colIndex].m_shapeType==SHAPE_CONVEX_HULL); + __global const b3ConvexPolyhedronData_t* hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex]; + + b3Float4 pInA; + if (logme) + { + + + // b3Float4 supVec = b3MakeFloat4(0,0,0,0); + float maxDot = -B3_LARGE_FLOAT; + + if( 0 < hull->m_numVertices ) + { + const b3Float4 scaled = localDir; + int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot); + pInA = cpuVertices[hull->m_vertexOffset+index]; + + } + + + } else + { + pInA = b3LocalGetSupportVertex(localDir,hull,cpuVertices); + } + + //move vertex to world space + *outp = b3TransformPoint(pInA,pos,orn); + +} + +inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + const b3Float4* _dir, b3MprSupport_t *supp) +{ + b3Float4 dir; + dir = *_dir; + b3MprConvexSupport(pairIndex,bodyIndexA,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v1,0); + dir = *_dir*-1.f; + b3MprConvexSupport(pairIndex,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v2,0); + supp->v = supp->v1 - supp->v2; +} + + + + + + + + + +inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center) +{ + + center->v1 = cpuBodyBuf[bodyIndexA].m_pos; + center->v2 = cpuBodyBuf[bodyIndexB].m_pos; + center->v = center->v1 - center->v2; +} + +inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z) +{ + (*v).x = x; + (*v).y = y; + (*v).z = z; + (*v).w = 0.f; +} + +inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w) +{ + (*v).x += (*w).x; + (*v).y += (*w).y; + (*v).z += (*w).z; +} + +inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w) +{ + *v = *w; +} + +inline void b3MprVec3Scale(b3Float4 *d, float k) +{ + *d *= k; +} + +inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b) +{ + float dot; + + dot = b3Dot3F4(*a,*b); + return dot; +} + + +inline float b3MprVec3Len2(const b3Float4 *v) +{ + return b3MprVec3Dot(v, v); +} + +inline void b3MprVec3Normalize(b3Float4 *d) +{ + float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d)); + b3MprVec3Scale(d, k); +} + +inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b) +{ + *d = b3Cross3(*a,*b); + +} + + +inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w) +{ + *d = *v - *w; +} + +inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir) +{ + b3Float4 v2v1, v3v1; + + b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 1)->v); + b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 1)->v); + b3MprVec3Cross(dir, &v2v1, &v3v1); + b3MprVec3Normalize(dir); +} + + +inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal, + const b3Float4 *dir) +{ + float dot; + dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v); + return b3MprIsZero(dot) || dot > 0.f; +} + +inline int portalReachTolerance(const b3MprSimplex_t *portal, + const b3MprSupport_t *v4, + const b3Float4 *dir) +{ + float dv1, dv2, dv3, dv4; + float dot1, dot2, dot3; + + // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} + + dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir); + dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir); + dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir); + dv4 = b3MprVec3Dot(&v4->v, dir); + + dot1 = dv4 - dv1; + dot2 = dv4 - dv2; + dot3 = dv4 - dv3; + + dot1 = B3_MPR_FMIN(dot1, dot2); + dot1 = B3_MPR_FMIN(dot1, dot3); + + return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE; +} + +inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal, + const b3MprSupport_t *v4, + const b3Float4 *dir) +{ + float dot; + dot = b3MprVec3Dot(&v4->v, dir); + return b3MprIsZero(dot) || dot > 0.f; +} + +inline void b3ExpandPortal(b3MprSimplex_t *portal, + const b3MprSupport_t *v4) +{ + float dot; + b3Float4 v4v0; + + b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v); + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0); + if (dot > 0.f){ + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0); + if (dot > 0.f){ + b3MprSimplexSet(portal, 1, v4); + }else{ + b3MprSimplexSet(portal, 3, v4); + } + }else{ + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0); + if (dot > 0.f){ + b3MprSimplexSet(portal, 2, v4); + }else{ + b3MprSimplexSet(portal, 1, v4); + } + } +} + + + +B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + __global int* hasSepAxis, + b3MprSimplex_t *portal) +{ + b3Float4 dir, va, vb; + float dot; + int cont; + + + + // vertex 0 is center of portal + b3FindOrigin(bodyIndexA,bodyIndexB,cpuBodyBuf, b3MprSimplexPointW(portal, 0)); + // vertex 0 is center of portal + b3MprSimplexSetSize(portal, 1); + + + + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){ + // Portal's center lies on origin (0,0,0) => we know that objects + // intersect but we would need to know penetration info. + // So move center little bit... + b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f); + b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va); + } + + + // vertex 1 = support in direction of origin + b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Scale(&dir, -1.f); + b3MprVec3Normalize(&dir); + + + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 1)); + + b3MprSimplexSetSize(portal, 2); + + // test if origin isn't outside of v1 + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir); + + + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + + // vertex 2 + b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v, + &b3MprSimplexPoint(portal, 1)->v); + if (b3MprIsZero(b3MprVec3Len2(&dir))){ + if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){ + // origin lies on v1 + return 1; + }else{ + // origin lies on v0-v1 segment + return 2; + } + } + + b3MprVec3Normalize(&dir); + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 2)); + + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir); + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + b3MprSimplexSetSize(portal, 3); + + // vertex 3 direction + b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Cross(&dir, &va, &vb); + b3MprVec3Normalize(&dir); + + // it is better to form portal faces to be oriented "outside" origin + dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v); + if (dot > 0.f){ + b3MprSimplexSwap(portal, 1, 2); + b3MprVec3Scale(&dir, -1.f); + } + + while (b3MprSimplexSize(portal) < 4){ + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 3)); + + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir); + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + cont = 0; + + // test if origin is outside (v1, v0, v3) - set v2 as v3 and + // continue + b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 3)->v); + dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !b3MprIsZero(dot)){ + b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3)); + cont = 1; + } + + if (!cont){ + // test if origin is outside (v3, v0, v2) - set v1 as v3 and + // continue + b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 2)->v); + dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !b3MprIsZero(dot)){ + b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3)); + cont = 1; + } + } + + if (cont){ + b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Cross(&dir, &va, &vb); + b3MprVec3Normalize(&dir); + }else{ + b3MprSimplexSetSize(portal, 4); + } + } + + return 0; +} + + +B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + b3MprSimplex_t *portal) +{ + b3Float4 dir; + b3MprSupport_t v4; + + for (int i=0;iv, + &b3MprSimplexPoint(portal, 2)->v); + b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 2)->v); + b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v); + + sum = b[0] + b[1] + b[2] + b[3]; + + if (b3MprIsZero(sum) || sum < 0.f){ + b[0] = 0.f; + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 3)->v); + b[1] = b3MprVec3Dot(&vec, &dir); + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[2] = b3MprVec3Dot(&vec, &dir); + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 2)->v); + b[3] = b3MprVec3Dot(&vec, &dir); + + sum = b[1] + b[2] + b[3]; + } + + inv = 1.f / sum; + + b3MprVec3Copy(&p1, b3mpr_vec3_origin); + b3MprVec3Copy(&p2, b3mpr_vec3_origin); + for (i = 0; i < 4; i++){ + b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1); + b3MprVec3Scale(&vec, b[i]); + b3MprVec3Add(&p1, &vec); + + b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2); + b3MprVec3Scale(&vec, b[i]); + b3MprVec3Add(&p2, &vec); + } + b3MprVec3Scale(&p1, inv); + b3MprVec3Scale(&p2, inv); + + b3MprVec3Copy(pos, &p1); + b3MprVec3Add(pos, &p2); + b3MprVec3Scale(pos, 0.5); +} + +inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b) +{ + b3Float4 ab; + b3MprVec3Sub2(&ab, a, b); + return b3MprVec3Len2(&ab); +} + +inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P, + const b3Float4 *x0, + const b3Float4 *b, + b3Float4 *witness) +{ + // The computation comes from solving equation of segment: + // S(t) = x0 + t.d + // where - x0 is initial point of segment + // - d is direction of segment from x0 (|d| > 0) + // - t belongs to <0, 1> interval + // + // Than, distance from a segment to some point P can be expressed: + // D(t) = |x0 + t.d - P|^2 + // which is distance from any point on segment. Minimization + // of this function brings distance from P to segment. + // Minimization of D(t) leads to simple quadratic equation that's + // solving is straightforward. + // + // Bonus of this method is witness point for free. + + float dist, t; + b3Float4 d, a; + + // direction of segment + b3MprVec3Sub2(&d, b, x0); + + // precompute vector from P to x0 + b3MprVec3Sub2(&a, x0, P); + + t = -1.f * b3MprVec3Dot(&a, &d); + t /= b3MprVec3Len2(&d); + + if (t < 0.f || b3MprIsZero(t)){ + dist = b3MprVec3Dist2(x0, P); + if (witness) + b3MprVec3Copy(witness, x0); + }else if (t > 1.f || b3MprEq(t, 1.f)){ + dist = b3MprVec3Dist2(b, P); + if (witness) + b3MprVec3Copy(witness, b); + }else{ + if (witness){ + b3MprVec3Copy(witness, &d); + b3MprVec3Scale(witness, t); + b3MprVec3Add(witness, x0); + dist = b3MprVec3Dist2(witness, P); + }else{ + // recycling variables + b3MprVec3Scale(&d, t); + b3MprVec3Add(&d, &a); + dist = b3MprVec3Len2(&d); + } + } + + return dist; +} + + +inline float b3MprVec3PointTriDist2(const b3Float4 *P, + const b3Float4 *x0, const b3Float4 *B, + const b3Float4 *C, + b3Float4 *witness) +{ + // Computation comes from analytic expression for triangle (x0, B, C) + // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and + // Then equation for distance is: + // D(s, t) = | T(s, t) - P |^2 + // This leads to minimization of quadratic function of two variables. + // The solution from is taken only if s is between 0 and 1, t is + // between 0 and 1 and t + s < 1, otherwise distance from segment is + // computed. + + b3Float4 d1, d2, a; + float u, v, w, p, q, r; + float s, t, dist, dist2; + b3Float4 witness2; + + b3MprVec3Sub2(&d1, B, x0); + b3MprVec3Sub2(&d2, C, x0); + b3MprVec3Sub2(&a, x0, P); + + u = b3MprVec3Dot(&a, &a); + v = b3MprVec3Dot(&d1, &d1); + w = b3MprVec3Dot(&d2, &d2); + p = b3MprVec3Dot(&a, &d1); + q = b3MprVec3Dot(&a, &d2); + r = b3MprVec3Dot(&d1, &d2); + + s = (q * r - w * p) / (w * v - r * r); + t = (-s * r - q) / w; + + if ((b3MprIsZero(s) || s > 0.f) + && (b3MprEq(s, 1.f) || s < 1.f) + && (b3MprIsZero(t) || t > 0.f) + && (b3MprEq(t, 1.f) || t < 1.f) + && (b3MprEq(t + s, 1.f) || t + s < 1.f)){ + + if (witness){ + b3MprVec3Scale(&d1, s); + b3MprVec3Scale(&d2, t); + b3MprVec3Copy(witness, x0); + b3MprVec3Add(witness, &d1); + b3MprVec3Add(witness, &d2); + + dist = b3MprVec3Dist2(witness, P); + }else{ + dist = s * s * v; + dist += t * t * w; + dist += 2.f * s * t * r; + dist += 2.f * s * p; + dist += 2.f * t * q; + dist += u; + } + }else{ + dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness); + + dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + b3MprVec3Copy(witness, &witness2); + } + + dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + b3MprVec3Copy(witness, &witness2); + } + } + + return dist; +} + + +B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + b3MprSimplex_t *portal, + float *depth, b3Float4 *pdir, b3Float4 *pos) +{ + b3Float4 dir; + b3MprSupport_t v4; + unsigned long iterations; + + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + + iterations = 1UL; + for (int i=0;i find penetration info + if (portalReachTolerance(portal, &v4, &dir) + || iterations ==B3_MPR_MAX_ITERATIONS) + { + *depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin,&b3MprSimplexPoint(portal, 1)->v,&b3MprSimplexPoint(portal, 2)->v,&b3MprSimplexPoint(portal, 3)->v,pdir); + *depth = B3_MPR_SQRT(*depth); + + if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z)) + { + + *pdir = dir; + } + b3MprVec3Normalize(pdir); + + // barycentric coordinates: + b3FindPos(portal, pos); + + + return; + } + + b3ExpandPortal(portal, &v4); + + iterations++; + } +} + +B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos) +{ + // Touching contact on portal's v1 - so depth is zero and direction + // is unimportant and pos can be guessed + *depth = 0.f; + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + + b3MprVec3Copy(dir, b3mpr_vec3_origin); + + b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1); + b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2); + b3MprVec3Scale(pos, 0.5); +} + +B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal, + float *depth, b3Float4 *dir, b3Float4 *pos) +{ + + // Origin lies on v0-v1 segment. + // Depth is distance to v1, direction also and position must be + // computed + + b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1); + b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2); + b3MprVec3Scale(pos, 0.5f); + + + b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v); + *depth = B3_MPR_SQRT(b3MprVec3Len2(dir)); + b3MprVec3Normalize(dir); +} + + + +inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB, + b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + __global int* hasSepAxis, + float *depthOut, b3Float4* dirOut, b3Float4* posOut) +{ + + b3MprSimplex_t portal; + + +// if (!hasSepAxis[pairIndex]) + // return -1; + + hasSepAxis[pairIndex] = 0; + int res; + + // Phase 1: Portal discovery + res = b3DiscoverPortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,hasSepAxis, &portal); + + + //sepAxis[pairIndex] = *pdir;//or -dir? + + switch (res) + { + case 0: + { + // Phase 2: Portal refinement + + res = b3RefinePortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal); + if (res < 0) + return -1; + + // Phase 3. Penetration info + b3FindPenetr(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal, depthOut, dirOut, posOut); + hasSepAxis[pairIndex] = 1; + sepAxis[pairIndex] = -*dirOut; + break; + } + case 1: + { + // Touching contact on portal's v1. + b3FindPenetrTouch(&portal, depthOut, dirOut, posOut); + break; + } + case 2: + { + + b3FindPenetrSegment( &portal, depthOut, dirOut, posOut); + break; + } + default: + { + hasSepAxis[pairIndex]=0; + //if (res < 0) + //{ + // Origin isn't inside portal - no collision. + return -1; + //} + } + }; + + return 0; +}; + + + +#endif //B3_MPR_PENETRATION_H diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h new file mode 100644 index 000000000000..718222ebcae9 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h @@ -0,0 +1,196 @@ + +#ifndef B3_NEW_CONTACT_REDUCTION_H +#define B3_NEW_CONTACT_REDUCTION_H + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +#define GET_NPOINTS(x) (x).m_worldNormalOnB.w + + +int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx) +{ + if( nPoints == 0 ) + return 0; + + if (nPoints <=4) + return nPoints; + + + if (nPoints >64) + nPoints = 64; + + b3Float4 center = b3MakeFloat4(0,0,0,0); + { + + for (int i=0;i0) + { + + __global b3Float4* pointsIn = &worldVertsB2[pairIndex*vertexFaceCapacity]; + b3Float4 normal = -separatingNormals[i]; + + int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx); + + int dstIdx; + dstIdx = b3AtomicInc( nGlobalContactsOut); + +//#if 0 + b3Assert(dstIdx < contactCapacity); + if (dstIdx < contactCapacity) + { + + __global struct b3Contact4Data* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = -normal; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + int bodyA = pairs[pairIndex].x; + int bodyB = pairs[pairIndex].y; + + pairs[pairIndex].w = dstIdx; + + c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB; + c->m_childIndexA =-1; + c->m_childIndexB =-1; + + switch (nReducedContacts) + { + case 4: + c->m_worldPosB[3] = pointsIn[contactIdx.w]; + case 3: + c->m_worldPosB[2] = pointsIn[contactIdx.z]; + case 2: + c->m_worldPosB[1] = pointsIn[contactIdx.y]; + case 1: + c->m_worldPosB[0] = pointsIn[contactIdx.x]; + default: + { + } + }; + + GET_NPOINTS(*c) = nReducedContacts; + + } + + +//#endif + + }// if (numContactsOut>0) + }// if (hasSeparatingAxis[i]) + }// if (im_escapeIndexOrTriangleIndex&~(y)); +} + +inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode) +{ + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0; +} + +inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode) +{ + return -rootNode->m_escapeIndexOrTriangleIndex; +} + +inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2,int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization) +{ + b3Float4 clampedPoint = b3MaxFloat4(point2,bvhAabbMin); + clampedPoint = b3MinFloat4 (clampedPoint, bvhAabbMax); + + b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization; + if (isMax) + { + out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1)); + out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1)); + out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1)); + } else + { + out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe)); + out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe)); + out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe)); + } + +} + + +inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow( + const unsigned short int* aabbMin1, + const unsigned short int* aabbMax1, + const unsigned short int* aabbMin2, + const unsigned short int* aabbMax2) +{ + //int overlap = 1; + if (aabbMin1[0] > aabbMax2[0]) + return 0; + if (aabbMax1[0] < aabbMin2[0]) + return 0; + if (aabbMin1[1] > aabbMax2[1]) + return 0; + if (aabbMax1[1] < aabbMin2[1]) + return 0; + if (aabbMin1[2] > aabbMax2[2]) + return 0; + if (aabbMax1[2] < aabbMin2[2]) + return 0; + return 1; + //overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap; + //overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap; + //overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap; + //return overlap; +} + + +#endif //B3_QUANTIZED_BVH_NODE_H diff --git a/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h new file mode 100644 index 000000000000..35b51970063e --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h @@ -0,0 +1,97 @@ +#ifndef B3_REDUCE_CONTACTS_H +#define B3_REDUCE_CONTACTS_H + +inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx) +{ + if( nPoints == 0 ) + return 0; + + if (nPoints <=4) + return nPoints; + + + if (nPoints >64) + nPoints = 64; + + b3Float4 center = b3MakeFloat4(0,0,0,0); + { + + for (int i=0;im_pos; + b3Quat orientation = body->m_quat; + + int collidableIndex = body->m_collidableIdx; + int shapeIndex = collidables[collidableIndex].m_shapeIndex; + + if (shapeIndex>=0) + { + + b3Aabb_t localAabb = localShapeAABB[collidableIndex]; + b3Aabb_t worldAabb; + + b3Float4 aabbAMinOut,aabbAMaxOut; + float margin = 0.f; + b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&aabbAMinOut,&aabbAMaxOut); + + worldAabb.m_minVec =aabbAMinOut; + worldAabb.m_minIndices[3] = bodyId; + worldAabb.m_maxVec = aabbAMaxOut; + worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass==0.f? 0 : 1; + worldAabbs[bodyId] = worldAabb; + } +} + +#endif //B3_UPDATE_AABBS_H diff --git a/extern/bullet/src/Bullet3Collision/premake4.lua b/extern/bullet/src/Bullet3Collision/premake4.lua new file mode 100644 index 000000000000..bd0da541ec19 --- /dev/null +++ b/extern/bullet/src/Bullet3Collision/premake4.lua @@ -0,0 +1,16 @@ + project "Bullet3Collision" + + language "C++" + + kind "StaticLib" + + includedirs {".."} + + if os.is("Linux") then + buildoptions{"-fPIC"} + end + + files { + "**.cpp", + "**.h" + } \ No newline at end of file diff --git a/extern/bullet/src/Bullet3Common/CMakeLists.txt b/extern/bullet/src/Bullet3Common/CMakeLists.txt new file mode 100644 index 000000000000..e899e67d944e --- /dev/null +++ b/extern/bullet/src/Bullet3Common/CMakeLists.txt @@ -0,0 +1,63 @@ + +INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/src +) + +SET(Bullet3Common_SRCS + b3AlignedAllocator.cpp + b3Vector3.cpp + b3Logging.cpp +) + +SET(Bullet3Common_HDRS + b3AlignedAllocator.h + b3AlignedObjectArray.h + b3CommandLineArgs.h + b3HashMap.h + b3Logging.h + b3Matrix3x3.h + b3MinMax.h + b3PoolAllocator.h + b3QuadWord.h + b3Quaternion.h + b3Random.h + b3Scalar.h + b3StackAlloc.h + b3Transform.h + b3TransformUtil.h + b3Vector3.h + shared/b3Float4 + shared/b3Int2.h + shared/b3Int4.h + shared/b3Mat3x3.h + shared/b3PlatformDefinitions + shared/b3Quat.h +) + +ADD_LIBRARY(Bullet3Common ${Bullet3Common_SRCS} ${Bullet3Common_HDRS}) +SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES SOVERSION ${BULLET_VERSION}) + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + #FILES_MATCHING requires CMake 2.6 + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS Bullet3Common DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS Bullet3Common + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN +".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES PUBLIC_HEADER "${Bullet3Common_HDRS}") + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/extern/bullet/src/Bullet3Common/b3AlignedAllocator.cpp b/extern/bullet/src/Bullet3Common/b3AlignedAllocator.cpp new file mode 100644 index 000000000000..b98e2b4d33ec --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3AlignedAllocator.cpp @@ -0,0 +1,181 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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 "b3AlignedAllocator.h" + +int b3g_numAlignedAllocs = 0; +int b3g_numAlignedFree = 0; +int b3g_totalBytesAlignedAllocs = 0;//detect memory leaks + +static void *b3AllocDefault(size_t size) +{ + return malloc(size); +} + +static void b3FreeDefault(void *ptr) +{ + free(ptr); +} + +static b3AllocFunc* b3s_allocFunc = b3AllocDefault; +static b3FreeFunc* b3s_freeFunc = b3FreeDefault; + + + +#if defined (B3_HAS_ALIGNED_ALLOCATOR) +#include +static void *b3AlignedAllocDefault(size_t size, int alignment) +{ + return _aligned_malloc(size, (size_t)alignment); +} + +static void b3AlignedFreeDefault(void *ptr) +{ + _aligned_free(ptr); +} +#elif defined(__CELLOS_LV2__) +#include + +static inline void *b3AlignedAllocDefault(size_t size, int alignment) +{ + return memalign(alignment, size); +} + +static inline void b3AlignedFreeDefault(void *ptr) +{ + free(ptr); +} +#else + + + + + +static inline void *b3AlignedAllocDefault(size_t size, int alignment) +{ + void *ret; + char *real; + real = (char *)b3s_allocFunc(size + sizeof(void *) + (alignment-1)); + if (real) { + ret = b3AlignPointer(real + sizeof(void *),alignment); + *((void **)(ret)-1) = (void *)(real); + } else { + ret = (void *)(real); + } + return (ret); +} + +static inline void b3AlignedFreeDefault(void *ptr) +{ + void* real; + + if (ptr) { + real = *((void **)(ptr)-1); + b3s_freeFunc(real); + } +} +#endif + + +static b3AlignedAllocFunc* b3s_alignedAllocFunc = b3AlignedAllocDefault; +static b3AlignedFreeFunc* b3s_alignedFreeFunc = b3AlignedFreeDefault; + +void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc) +{ + b3s_alignedAllocFunc = allocFunc ? allocFunc : b3AlignedAllocDefault; + b3s_alignedFreeFunc = freeFunc ? freeFunc : b3AlignedFreeDefault; +} + +void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc) +{ + b3s_allocFunc = allocFunc ? allocFunc : b3AllocDefault; + b3s_freeFunc = freeFunc ? freeFunc : b3FreeDefault; +} + +#ifdef B3_DEBUG_MEMORY_ALLOCATIONS +//this generic allocator provides the total allocated number of bytes +#include + +void* b3AlignedAllocInternal (size_t size, int alignment,int line,char* filename) +{ + void *ret; + char *real; + + b3g_totalBytesAlignedAllocs += size; + b3g_numAlignedAllocs++; + + + real = (char *)b3s_allocFunc(size + 2*sizeof(void *) + (alignment-1)); + if (real) { + ret = (void*) b3AlignPointer(real + 2*sizeof(void *), alignment); + *((void **)(ret)-1) = (void *)(real); + *((int*)(ret)-2) = size; + + } else { + ret = (void *)(real);//?? + } + + b3Printf("allocation#%d at address %x, from %s,line %d, size %d\n",b3g_numAlignedAllocs,real, filename,line,size); + + int* ptr = (int*)ret; + *ptr = 12; + return (ret); +} + +void b3AlignedFreeInternal (void* ptr,int line,char* filename) +{ + + void* real; + b3g_numAlignedFree++; + + if (ptr) { + real = *((void **)(ptr)-1); + int size = *((int*)(ptr)-2); + b3g_totalBytesAlignedAllocs -= size; + + b3Printf("free #%d at address %x, from %s,line %d, size %d\n",b3g_numAlignedFree,real, filename,line,size); + + b3s_freeFunc(real); + } else + { + b3Printf("NULL ptr\n"); + } +} + +#else //B3_DEBUG_MEMORY_ALLOCATIONS + +void* b3AlignedAllocInternal (size_t size, int alignment) +{ + b3g_numAlignedAllocs++; + void* ptr; + ptr = b3s_alignedAllocFunc(size, alignment); +// b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr); + return ptr; +} + +void b3AlignedFreeInternal (void* ptr) +{ + if (!ptr) + { + return; + } + + b3g_numAlignedFree++; +// b3Printf("b3AlignedFreeInternal %x\n",ptr); + b3s_alignedFreeFunc(ptr); +} + +#endif //B3_DEBUG_MEMORY_ALLOCATIONS + diff --git a/extern/bullet/src/Bullet3Common/b3AlignedAllocator.h b/extern/bullet/src/Bullet3Common/b3AlignedAllocator.h new file mode 100644 index 000000000000..be418bd55f94 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3AlignedAllocator.h @@ -0,0 +1,107 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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. +*/ + +#ifndef B3_ALIGNED_ALLOCATOR +#define B3_ALIGNED_ALLOCATOR + +///we probably replace this with our own aligned memory allocator +///so we replace _aligned_malloc and _aligned_free with our own +///that is better portable and more predictable + +#include "b3Scalar.h" +//#define B3_DEBUG_MEMORY_ALLOCATIONS 1 +#ifdef B3_DEBUG_MEMORY_ALLOCATIONS + +#define b3AlignedAlloc(a,b) \ + b3AlignedAllocInternal(a,b,__LINE__,__FILE__) + +#define b3AlignedFree(ptr) \ + b3AlignedFreeInternal(ptr,__LINE__,__FILE__) + +void* b3AlignedAllocInternal (size_t size, int alignment,int line,char* filename); + +void b3AlignedFreeInternal (void* ptr,int line,char* filename); + +#else + void* b3AlignedAllocInternal (size_t size, int alignment); + void b3AlignedFreeInternal (void* ptr); + + #define b3AlignedAlloc(size,alignment) b3AlignedAllocInternal(size,alignment) + #define b3AlignedFree(ptr) b3AlignedFreeInternal(ptr) + +#endif +typedef int btSizeType; + +typedef void *(b3AlignedAllocFunc)(size_t size, int alignment); +typedef void (b3AlignedFreeFunc)(void *memblock); +typedef void *(b3AllocFunc)(size_t size); +typedef void (b3FreeFunc)(void *memblock); + +///The developer can let all Bullet memory allocations go through a custom memory allocator, using b3AlignedAllocSetCustom +void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc); +///If the developer has already an custom aligned allocator, then b3AlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it. +void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc); + + +///The b3AlignedAllocator is a portable class for aligned memory allocations. +///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using b3AlignedAllocSetCustom and b3AlignedAllocSetCustomAligned. +template < typename T , unsigned Alignment > +class b3AlignedAllocator { + + typedef b3AlignedAllocator< T , Alignment > self_type; + +public: + + //just going down a list: + b3AlignedAllocator() {} + /* + b3AlignedAllocator( const self_type & ) {} + */ + + template < typename Other > + b3AlignedAllocator( const b3AlignedAllocator< Other , Alignment > & ) {} + + typedef const T* const_pointer; + typedef const T& const_reference; + typedef T* pointer; + typedef T& reference; + typedef T value_type; + + pointer address ( reference ref ) const { return &ref; } + const_pointer address ( const_reference ref ) const { return &ref; } + pointer allocate ( btSizeType n , const_pointer * hint = 0 ) { + (void)hint; + return reinterpret_cast< pointer >(b3AlignedAlloc( sizeof(value_type) * n , Alignment )); + } + void construct ( pointer ptr , const value_type & value ) { new (ptr) value_type( value ); } + void deallocate( pointer ptr ) { + b3AlignedFree( reinterpret_cast< void * >( ptr ) ); + } + void destroy ( pointer ptr ) { ptr->~value_type(); } + + + template < typename O > struct rebind { + typedef b3AlignedAllocator< O , Alignment > other; + }; + template < typename O > + self_type & operator=( const b3AlignedAllocator< O , Alignment > & ) { return *this; } + + friend bool operator==( const self_type & , const self_type & ) { return true; } +}; + + + +#endif //B3_ALIGNED_ALLOCATOR + diff --git a/extern/bullet/src/Bullet3Common/b3AlignedObjectArray.h b/extern/bullet/src/Bullet3Common/b3AlignedObjectArray.h new file mode 100644 index 000000000000..ef71016565ac --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3AlignedObjectArray.h @@ -0,0 +1,541 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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. +*/ + + +#ifndef B3_OBJECT_ARRAY__ +#define B3_OBJECT_ARRAY__ + +#include "b3Scalar.h" // has definitions like B3_FORCE_INLINE +#include "b3AlignedAllocator.h" + +///If the platform doesn't support placement new, you can disable B3_USE_PLACEMENT_NEW +///then the b3AlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors +///You can enable B3_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator= +///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and +///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240 + +#define B3_USE_PLACEMENT_NEW 1 +//#define B3_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in or or otherwise... +#define B3_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful + +#ifdef B3_USE_MEMCPY +#include +#include +#endif //B3_USE_MEMCPY + +#ifdef B3_USE_PLACEMENT_NEW +#include //for placement new +#endif //B3_USE_PLACEMENT_NEW + + +///The b3AlignedObjectArray template class uses a subset of the stl::vector interface for its methods +///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data +template +//template +class b3AlignedObjectArray +{ + b3AlignedAllocator m_allocator; + + int m_size; + int m_capacity; + T* m_data; + //PCK: added this line + bool m_ownsMemory; + +#ifdef B3_ALLOW_ARRAY_COPY_OPERATOR +public: + B3_FORCE_INLINE b3AlignedObjectArray& operator=(const b3AlignedObjectArray &other) + { + copyFromArray(other); + return *this; + } +#else//B3_ALLOW_ARRAY_COPY_OPERATOR +private: + B3_FORCE_INLINE b3AlignedObjectArray& operator=(const b3AlignedObjectArray &other); +#endif//B3_ALLOW_ARRAY_COPY_OPERATOR + +protected: + B3_FORCE_INLINE int allocSize(int size) + { + return (size ? size*2 : 1); + } + B3_FORCE_INLINE void copy(int start,int end, T* dest) const + { + int i; + for (i=start;i=0); + b3Assert(n=0); + b3Assert(n=0); + b3Assert(n=0); + b3Assert(n0); + m_size--; + m_data[m_size].~T(); + } + + + ///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument. + ///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations. + B3_FORCE_INLINE void resizeNoInitialize(int newsize) + { + int curSize = size(); + + if (newsize < curSize) + { + } else + { + if (newsize > size()) + { + reserve(newsize); + } + //leave this uninitialized + } + m_size = newsize; + } + + B3_FORCE_INLINE void resize(int newsize, const T& fillData=T()) + { + int curSize = size(); + + if (newsize < curSize) + { + for(int i = newsize; i < curSize; i++) + { + m_data[i].~T(); + } + } else + { + if (newsize > size()) + { + reserve(newsize); + } +#ifdef B3_USE_PLACEMENT_NEW + for (int i=curSize;i + void quickSortInternal(const L& CompareFunc,int lo, int hi) + { + // lo is the lower index, hi is the upper index + // of the region of array a that is to be sorted + int i=lo, j=hi; + T x=m_data[(lo+hi)/2]; + + // partition + do + { + while (CompareFunc(m_data[i],x)) + i++; + while (CompareFunc(x,m_data[j])) + j--; + if (i<=j) + { + swap(i,j); + i++; j--; + } + } while (i<=j); + + // recursion + if (lo + void quickSort(const L& CompareFunc) + { + //don't sort 0 or 1 elements + if (size()>1) + { + quickSortInternal(CompareFunc,0,size()-1); + } + } + + + ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/ + template + void downHeap(T *pArr, int k, int n, const L& CompareFunc) + { + /* PRE: a[k+1..N] is a heap */ + /* POST: a[k..N] is a heap */ + + T temp = pArr[k - 1]; + /* k has child(s) */ + while (k <= n/2) + { + int child = 2*k; + + if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child])) + { + child++; + } + /* pick larger child */ + if (CompareFunc(temp , pArr[child - 1])) + { + /* move child up */ + pArr[k - 1] = pArr[child - 1]; + k = child; + } + else + { + break; + } + } + pArr[k - 1] = temp; + } /*downHeap*/ + + void swap(int index0,int index1) + { +#ifdef B3_USE_MEMCPY + char temp[sizeof(T)]; + memcpy(temp,&m_data[index0],sizeof(T)); + memcpy(&m_data[index0],&m_data[index1],sizeof(T)); + memcpy(&m_data[index1],temp,sizeof(T)); +#else + T temp = m_data[index0]; + m_data[index0] = m_data[index1]; + m_data[index1] = temp; +#endif //B3_USE_PLACEMENT_NEW + + } + + template + void heapSort(const L& CompareFunc) + { + /* sort a[0..N-1], N.B. 0 to N-1 */ + int k; + int n = m_size; + for (k = n/2; k > 0; k--) + { + downHeap(m_data, k, n, CompareFunc); + } + + /* a[1..N] is now a heap */ + while ( n>=1 ) + { + swap(0,n-1); /* largest of a[0..n-1] */ + + + n = n - 1; + /* restore a[1..i-1] heap */ + downHeap(m_data, 1, n, CompareFunc); + } + } + + ///non-recursive binary search, assumes sorted array + int findBinarySearch(const T& key) const + { + int first = 0; + int last = size()-1; + + //assume sorted array + while (first <= last) { + int mid = (first + last) / 2; // compute mid point. + if (key > m_data[mid]) + first = mid + 1; // repeat search in top half. + else if (key < m_data[mid]) + last = mid - 1; // repeat search in bottom half. + else + return mid; // found it. return position ///// + } + return size(); // failed to find key + } + + + int findLinearSearch(const T& key) const + { + int index=size(); + int i; + + for (i=0;i +#include +#include +#include +#include +class b3CommandLineArgs +{ +protected: + + std::map pairs; + +public: + + // Constructor + b3CommandLineArgs(int argc, char **argv) + { + addArgs(argc,argv); + } + + void addArgs(int argc, char**argv) + { + for (int i = 1; i < argc; i++) + { + std::string arg = argv[i]; + + if ((arg.length() < 2) || (arg[0] != '-') || (arg[1] != '-')) { + continue; + } + + std::string::size_type pos; + std::string key, val; + if ((pos = arg.find( '=')) == std::string::npos) { + key = std::string(arg, 2, arg.length() - 2); + val = ""; + } else { + key = std::string(arg, 2, pos - 2); + val = std::string(arg, pos + 1, arg.length() - 1); + } + + //only add new keys, don't replace existing + if(pairs.find(key) == pairs.end()) + { + pairs[key] = val; + } + } + } + + bool CheckCmdLineFlag(const char* arg_name) + { + std::map::iterator itr; + if ((itr = pairs.find(arg_name)) != pairs.end()) { + return true; + } + return false; + } + + template + bool GetCmdLineArgument(const char *arg_name, T &val); + + int ParsedArgc() + { + return pairs.size(); + } +}; + +template +inline bool b3CommandLineArgs::GetCmdLineArgument(const char *arg_name, T &val) +{ + std::map::iterator itr; + if ((itr = pairs.find(arg_name)) != pairs.end()) { + std::istringstream strstream(itr->second); + strstream >> val; + return true; + } + return false; +} + +template <> +inline bool b3CommandLineArgs::GetCmdLineArgument(const char* arg_name, char* &val) +{ + std::map::iterator itr; + if ((itr = pairs.find(arg_name)) != pairs.end()) { + + std::string s = itr->second; + val = (char*) malloc(sizeof(char) * (s.length() + 1)); + std::strcpy(val, s.c_str()); + return true; + } else { + val = NULL; + } + return false; +} + + +#endif //COMMAND_LINE_ARGS_H diff --git a/extern/bullet/src/Bullet3Common/b3FileUtils.h b/extern/bullet/src/Bullet3Common/b3FileUtils.h new file mode 100644 index 000000000000..b5e8225cf0b1 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3FileUtils.h @@ -0,0 +1,138 @@ +#ifndef B3_FILE_UTILS_H +#define B3_FILE_UTILS_H + +#include +#include "b3Scalar.h" +#include //ptrdiff_h +#include + +struct b3FileUtils +{ + b3FileUtils() + { + } + virtual ~b3FileUtils() + { + } + + static bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen) + { + FILE* f=0; + f = fopen(orgFileName,"rb"); + if (f) + { + //printf("original file found: [%s]\n", orgFileName); + sprintf(relativeFileName,"%s", orgFileName); + fclose(f); + return true; + } + + //printf("Trying various directories, relative to current working directory\n"); + const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"}; + int numPrefixes = sizeof(prefix)/sizeof(const char*); + + f=0; + bool fileFound = false; + + for (int i=0;!f && i0); + if (maxPathLength>0) + { + path[len] = 0; + } + } + return len; + } + + static char toLowerChar(const char t) + { + if (t>=(char)'A' && t<=(char)'Z') + return t + ((char)'a' - (char)'A'); + else + return t; + } + + + static void toLower(char* str) + { + int len=strlen(str); + for (int i=0;i + +///very basic hashable string implementation, compatible with b3HashMap +struct b3HashString +{ + std::string m_string; + unsigned int m_hash; + + B3_FORCE_INLINE unsigned int getHash()const + { + return m_hash; + } + + + b3HashString(const char* name) + :m_string(name) + { + + /* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */ + static const unsigned int InitialFNV = 2166136261u; + static const unsigned int FNVMultiple = 16777619u; + + /* Fowler / Noll / Vo (FNV) Hash */ + unsigned int hash = InitialFNV; + int len = m_string.length(); + for(int i = 0; i 0 ) + ret = 1 ; + + return( ret ); + } + + bool equals(const b3HashString& other) const + { + return (m_string == other.m_string); + } + +}; + + +const int B3_HASH_NULL=0xffffffff; + + +class b3HashInt +{ + int m_uid; +public: + b3HashInt(int uid) :m_uid(uid) + { + } + + int getUid1() const + { + return m_uid; + } + + void setUid1(int uid) + { + m_uid = uid; + } + + bool equals(const b3HashInt& other) const + { + return getUid1() == other.getUid1(); + } + //to our success + B3_FORCE_INLINE unsigned int getHash()const + { + int key = m_uid; + // Thomas Wang's hash + key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); + return key; + } +}; + + + +class b3HashPtr +{ + + union + { + const void* m_pointer; + int m_hashValues[2]; + }; + +public: + + b3HashPtr(const void* ptr) + :m_pointer(ptr) + { + } + + const void* getPointer() const + { + return m_pointer; + } + + bool equals(const b3HashPtr& other) const + { + return getPointer() == other.getPointer(); + } + + //to our success + B3_FORCE_INLINE unsigned int getHash()const + { + const bool VOID_IS_8 = ((sizeof(void*)==8)); + + int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0]; + + // Thomas Wang's hash + key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); + return key; + } + + +}; + + +template +class b3HashKeyPtr +{ + int m_uid; +public: + + b3HashKeyPtr(int uid) :m_uid(uid) + { + } + + int getUid1() const + { + return m_uid; + } + + bool equals(const b3HashKeyPtr& other) const + { + return getUid1() == other.getUid1(); + } + + //to our success + B3_FORCE_INLINE unsigned int getHash()const + { + int key = m_uid; + // Thomas Wang's hash + key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); + return key; + } + + +}; + + +template +class b3HashKey +{ + int m_uid; +public: + + b3HashKey(int uid) :m_uid(uid) + { + } + + int getUid1() const + { + return m_uid; + } + + bool equals(const b3HashKey& other) const + { + return getUid1() == other.getUid1(); + } + //to our success + B3_FORCE_INLINE unsigned int getHash()const + { + int key = m_uid; + // Thomas Wang's hash + key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); + return key; + } +}; + + +///The b3HashMap template class implements a generic and lightweight hashmap. +///A basic sample of how to use b3HashMap is located in Demos\BasicDemo\main.cpp +template +class b3HashMap +{ + +protected: + b3AlignedObjectArray m_hashTable; + b3AlignedObjectArray m_next; + + b3AlignedObjectArray m_valueArray; + b3AlignedObjectArray m_keyArray; + + void growTables(const Key& /*key*/) + { + int newCapacity = m_valueArray.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] = B3_HASH_NULL; + } + for (i = 0; i < newCapacity; ++i) + { + m_next[i] = B3_HASH_NULL; + } + + for(i=0;i= (unsigned int)m_hashTable.size()) + { + return B3_HASH_NULL; + } + + int index = m_hashTable[hash]; + while ((index != B3_HASH_NULL) && key.equals(m_keyArray[index]) == false) + { + index = m_next[index]; + } + return index; + } + + void clear() + { + m_hashTable.clear(); + m_next.clear(); + m_valueArray.clear(); + m_keyArray.clear(); + } + +}; + +#endif //B3_HASH_MAP_H diff --git a/extern/bullet/src/Bullet3Common/b3Logging.cpp b/extern/bullet/src/Bullet3Common/b3Logging.cpp new file mode 100644 index 000000000000..a8e950715551 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3Logging.cpp @@ -0,0 +1,160 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + +#include "b3Logging.h" + +#include +#include + +#ifdef _WIN32 +#include +#endif //_WIN32 + + +void b3PrintfFuncDefault(const char* msg) +{ +#ifdef _WIN32 + OutputDebugStringA(msg); +#endif + printf("%s",msg); + //is this portable? + fflush(stdout); +} + +void b3WarningMessageFuncDefault(const char* msg) +{ +#ifdef _WIN32 + OutputDebugStringA(msg); +#endif + printf("%s",msg); + //is this portable? + fflush(stdout); + +} + + +void b3ErrorMessageFuncDefault(const char* msg) +{ +#ifdef _WIN32 + OutputDebugStringA(msg); +#endif + printf("%s",msg); + + //is this portable? + fflush(stdout); + +} + + + +static b3PrintfFunc* b3s_printfFunc = b3PrintfFuncDefault; +static b3WarningMessageFunc* b3s_warningMessageFunc = b3WarningMessageFuncDefault; +static b3ErrorMessageFunc* b3s_errorMessageFunc = b3ErrorMessageFuncDefault; + + +///The developer can route b3Printf output using their own implementation +void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc) +{ + b3s_printfFunc = printfFunc; +} +void b3SetCustomWarningMessageFunc(b3PrintfFunc* warningMessageFunc) +{ + b3s_warningMessageFunc = warningMessageFunc; +} +void b3SetCustomErrorMessageFunc(b3PrintfFunc* errorMessageFunc) +{ + b3s_errorMessageFunc = errorMessageFunc; +} + +//#define B3_MAX_DEBUG_STRING_LENGTH 2048 +#define B3_MAX_DEBUG_STRING_LENGTH 32768 + + +void b3OutputPrintfVarArgsInternal(const char *str, ...) +{ + char strDebug[B3_MAX_DEBUG_STRING_LENGTH]={0}; + va_list argList; + va_start(argList, str); +#ifdef _MSC_VER + vsprintf_s(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList); +#else + vsnprintf(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList); +#endif + (b3s_printfFunc)(strDebug); + va_end(argList); +} +void b3OutputWarningMessageVarArgsInternal(const char *str, ...) +{ + char strDebug[B3_MAX_DEBUG_STRING_LENGTH]={0}; + va_list argList; + va_start(argList, str); +#ifdef _MSC_VER + vsprintf_s(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList); +#else + vsnprintf(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList); +#endif + (b3s_warningMessageFunc)(strDebug); + va_end(argList); +} +void b3OutputErrorMessageVarArgsInternal(const char *str, ...) +{ + + char strDebug[B3_MAX_DEBUG_STRING_LENGTH]={0}; + va_list argList; + va_start(argList, str); +#ifdef _MSC_VER + vsprintf_s(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList); +#else + vsnprintf(strDebug,B3_MAX_DEBUG_STRING_LENGTH,str,argList); +#endif + (b3s_errorMessageFunc)(strDebug); + va_end(argList); + +} + + +void b3EnterProfileZoneDefault(const char* name) +{ +} +void b3LeaveProfileZoneDefault() +{ +} +static b3EnterProfileZoneFunc* b3s_enterFunc = b3EnterProfileZoneDefault; +static b3LeaveProfileZoneFunc* b3s_leaveFunc = b3LeaveProfileZoneDefault; +void b3EnterProfileZone(const char* name) +{ + (b3s_enterFunc)(name); +} +void b3LeaveProfileZone() +{ + (b3s_leaveFunc)(); +} + +void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc) +{ + b3s_enterFunc = enterFunc; +} +void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc) +{ + b3s_leaveFunc = leaveFunc; +} + + + + +#ifndef _MSC_VER +#undef vsprintf_s +#endif + diff --git a/extern/bullet/src/Bullet3Common/b3Logging.h b/extern/bullet/src/Bullet3Common/b3Logging.h new file mode 100644 index 000000000000..b302effe4354 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3Logging.h @@ -0,0 +1,77 @@ + +#ifndef B3_LOGGING_H +#define B3_LOGGING_H + +#ifdef __cplusplus +extern "C" { +#endif + +///We add the do/while so that the statement "if (condition) b3Printf("test"); else {...}" would fail +///You can also customize the message by uncommenting out a different line below +#define b3Printf(...) b3OutputPrintfVarArgsInternal(__VA_ARGS__) +//#define b3Printf(...) do {b3OutputPrintfVarArgsInternal("b3Printf[%s,%d]:",__FILE__,__LINE__);b3OutputPrintfVarArgsInternal(__VA_ARGS__); } while(0) +//#define b3Printf b3OutputPrintfVarArgsInternal +//#define b3Printf(...) printf(__VA_ARGS__) +//#define b3Printf(...) + +#define b3Warning(...) do {b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n",__FILE__,__LINE__);b3OutputWarningMessageVarArgsInternal(__VA_ARGS__); }while(0) +#define b3Error(...) do {b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n",__FILE__,__LINE__);b3OutputErrorMessageVarArgsInternal(__VA_ARGS__); } while(0) + + +#ifndef B3_NO_PROFILE + +void b3EnterProfileZone(const char* name); +void b3LeaveProfileZone(); +#ifdef __cplusplus + +class b3ProfileZone +{ +public: + b3ProfileZone(const char* name) + { + b3EnterProfileZone( name ); + } + + ~b3ProfileZone() + { + b3LeaveProfileZone(); + } +}; + +#define B3_PROFILE( name ) b3ProfileZone __profile( name ) +#endif + +#else //B3_NO_PROFILE + +#define B3_PROFILE( name ) +#define b3StartProfile(a) +#define b3StopProfile + +#endif //#ifndef B3_NO_PROFILE + + +typedef void (b3PrintfFunc)(const char* msg); +typedef void (b3WarningMessageFunc)(const char* msg); +typedef void (b3ErrorMessageFunc)(const char* msg); +typedef void (b3EnterProfileZoneFunc)(const char* msg); +typedef void (b3LeaveProfileZoneFunc)(); + +///The developer can route b3Printf output using their own implementation +void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc); +void b3SetCustomWarningMessageFunc(b3WarningMessageFunc* warningMsgFunc); +void b3SetCustomErrorMessageFunc(b3ErrorMessageFunc* errorMsgFunc); + +///Set custom profile zone functions (zones can be nested) +void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc); +void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc); + +///Don't use those internal functions directly, use the b3Printf or b3SetCustomPrintfFunc instead (or warning/error version) +void b3OutputPrintfVarArgsInternal(const char *str, ...); +void b3OutputWarningMessageVarArgsInternal(const char *str, ...); +void b3OutputErrorMessageVarArgsInternal(const char *str, ...); + +#ifdef __cplusplus + } +#endif + +#endif//B3_LOGGING_H \ No newline at end of file diff --git a/extern/bullet/src/Bullet3Common/b3Matrix3x3.h b/extern/bullet/src/Bullet3Common/b3Matrix3x3.h new file mode 100644 index 000000000000..89b57cf59a94 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3Matrix3x3.h @@ -0,0 +1,1362 @@ +/* +Copyright (c) 2003-2013 Gino van den Bergen / 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. +*/ + + +#ifndef B3_MATRIX3x3_H +#define B3_MATRIX3x3_H + +#include "b3Vector3.h" +#include "b3Quaternion.h" +#include + +#ifdef B3_USE_SSE +//const __m128 B3_ATTRIBUTE_ALIGNED16(b3v2220) = {2.0f, 2.0f, 2.0f, 0.0f}; +const __m128 B3_ATTRIBUTE_ALIGNED16(b3vMPPP) = {-0.0f, +0.0f, +0.0f, +0.0f}; +#endif + +#if defined(B3_USE_SSE) || defined(B3_USE_NEON) +const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3v1000) = {1.0f, 0.0f, 0.0f, 0.0f}; +const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3v0100) = {0.0f, 1.0f, 0.0f, 0.0f}; +const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3v0010) = {0.0f, 0.0f, 1.0f, 0.0f}; +#endif + +#ifdef B3_USE_DOUBLE_PRECISION +#define b3Matrix3x3Data b3Matrix3x3DoubleData +#else +#define b3Matrix3x3Data b3Matrix3x3FloatData +#endif //B3_USE_DOUBLE_PRECISION + + +/**@brief The b3Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with b3Quaternion, b3Transform and b3Vector3. +* Make sure to only include a pure orthogonal matrix without scaling. */ +B3_ATTRIBUTE_ALIGNED16(class) b3Matrix3x3 { + + ///Data storage for the matrix, each vector is a row of the matrix + b3Vector3 m_el[3]; + +public: + /** @brief No initializaion constructor */ + b3Matrix3x3 () {} + + // explicit b3Matrix3x3(const b3Scalar *m) { setFromOpenGLSubMatrix(m); } + + /**@brief Constructor from Quaternion */ + explicit b3Matrix3x3(const b3Quaternion& q) { setRotation(q); } + /* + template + Matrix3x3(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll) + { + setEulerYPR(yaw, pitch, roll); + } + */ + /** @brief Constructor with row major formatting */ + b3Matrix3x3(const b3Scalar& xx, const b3Scalar& xy, const b3Scalar& xz, + const b3Scalar& yx, const b3Scalar& yy, const b3Scalar& yz, + const b3Scalar& zx, const b3Scalar& zy, const b3Scalar& zz) + { + setValue(xx, xy, xz, + yx, yy, yz, + zx, zy, zz); + } + +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON) + B3_FORCE_INLINE b3Matrix3x3 (const b3SimdFloat4 v0, const b3SimdFloat4 v1, const b3SimdFloat4 v2 ) + { + m_el[0].mVec128 = v0; + m_el[1].mVec128 = v1; + m_el[2].mVec128 = v2; + } + + B3_FORCE_INLINE b3Matrix3x3 (const b3Vector3& v0, const b3Vector3& v1, const b3Vector3& v2 ) + { + m_el[0] = v0; + m_el[1] = v1; + m_el[2] = v2; + } + + // Copy constructor + B3_FORCE_INLINE b3Matrix3x3(const b3Matrix3x3& rhs) + { + m_el[0].mVec128 = rhs.m_el[0].mVec128; + m_el[1].mVec128 = rhs.m_el[1].mVec128; + m_el[2].mVec128 = rhs.m_el[2].mVec128; + } + + // Assignment Operator + B3_FORCE_INLINE b3Matrix3x3& operator=(const b3Matrix3x3& m) + { + m_el[0].mVec128 = m.m_el[0].mVec128; + m_el[1].mVec128 = m.m_el[1].mVec128; + m_el[2].mVec128 = m.m_el[2].mVec128; + + return *this; + } + +#else + + /** @brief Copy constructor */ + B3_FORCE_INLINE b3Matrix3x3 (const b3Matrix3x3& other) + { + m_el[0] = other.m_el[0]; + m_el[1] = other.m_el[1]; + m_el[2] = other.m_el[2]; + } + + /** @brief Assignment Operator */ + B3_FORCE_INLINE b3Matrix3x3& operator=(const b3Matrix3x3& other) + { + m_el[0] = other.m_el[0]; + m_el[1] = other.m_el[1]; + m_el[2] = other.m_el[2]; + return *this; + } + +#endif + + /** @brief Get a column of the matrix as a vector + * @param i Column number 0 indexed */ + B3_FORCE_INLINE b3Vector3 getColumn(int i) const + { + return b3MakeVector3(m_el[0][i],m_el[1][i],m_el[2][i]); + } + + + /** @brief Get a row of the matrix as a vector + * @param i Row number 0 indexed */ + B3_FORCE_INLINE const b3Vector3& getRow(int i) const + { + b3FullAssert(0 <= i && i < 3); + return m_el[i]; + } + + /** @brief Get a mutable reference to a row of the matrix as a vector + * @param i Row number 0 indexed */ + B3_FORCE_INLINE b3Vector3& operator[](int i) + { + b3FullAssert(0 <= i && i < 3); + return m_el[i]; + } + + /** @brief Get a const reference to a row of the matrix as a vector + * @param i Row number 0 indexed */ + B3_FORCE_INLINE const b3Vector3& operator[](int i) const + { + b3FullAssert(0 <= i && i < 3); + return m_el[i]; + } + + /** @brief Multiply by the target matrix on the right + * @param m Rotation matrix to be applied + * Equivilant to this = this * m */ + b3Matrix3x3& operator*=(const b3Matrix3x3& m); + + /** @brief Adds by the target matrix on the right + * @param m matrix to be applied + * Equivilant to this = this + m */ + b3Matrix3x3& operator+=(const b3Matrix3x3& m); + + /** @brief Substractss by the target matrix on the right + * @param m matrix to be applied + * Equivilant to this = this - m */ + b3Matrix3x3& operator-=(const b3Matrix3x3& m); + + /** @brief Set from the rotational part of a 4x4 OpenGL matrix + * @param m A pointer to the beginning of the array of scalars*/ + void setFromOpenGLSubMatrix(const b3Scalar *m) + { + m_el[0].setValue(m[0],m[4],m[8]); + m_el[1].setValue(m[1],m[5],m[9]); + m_el[2].setValue(m[2],m[6],m[10]); + + } + /** @brief Set the values of the matrix explicitly (row major) + * @param xx Top left + * @param xy Top Middle + * @param xz Top Right + * @param yx Middle Left + * @param yy Middle Middle + * @param yz Middle Right + * @param zx Bottom Left + * @param zy Bottom Middle + * @param zz Bottom Right*/ + void setValue(const b3Scalar& xx, const b3Scalar& xy, const b3Scalar& xz, + const b3Scalar& yx, const b3Scalar& yy, const b3Scalar& yz, + const b3Scalar& zx, const b3Scalar& zy, const b3Scalar& zz) + { + m_el[0].setValue(xx,xy,xz); + m_el[1].setValue(yx,yy,yz); + m_el[2].setValue(zx,zy,zz); + } + + /** @brief Set the matrix from a quaternion + * @param q The Quaternion to match */ + void setRotation(const b3Quaternion& q) + { + b3Scalar d = q.length2(); + b3FullAssert(d != b3Scalar(0.0)); + b3Scalar s = b3Scalar(2.0) / d; + + #if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 vs, Q = q.get128(); + __m128i Qi = b3CastfTo128i(Q); + __m128 Y, Z; + __m128 V1, V2, V3; + __m128 V11, V21, V31; + __m128 NQ = _mm_xor_ps(Q, b3vMzeroMask); + __m128i NQi = b3CastfTo128i(NQ); + + V1 = b3CastiTo128f(_mm_shuffle_epi32 (Qi, B3_SHUFFLE(1,0,2,3))); // Y X Z W + V2 = _mm_shuffle_ps(NQ, Q, B3_SHUFFLE(0,0,1,3)); // -X -X Y W + V3 = b3CastiTo128f(_mm_shuffle_epi32 (Qi, B3_SHUFFLE(2,1,0,3))); // Z Y X W + V1 = _mm_xor_ps(V1, b3vMPPP); // change the sign of the first element + + V11 = b3CastiTo128f(_mm_shuffle_epi32 (Qi, B3_SHUFFLE(1,1,0,3))); // Y Y X W + V21 = _mm_unpackhi_ps(Q, Q); // Z Z W W + V31 = _mm_shuffle_ps(Q, NQ, B3_SHUFFLE(0,2,0,3)); // X Z -X -W + + V2 = V2 * V1; // + V1 = V1 * V11; // + V3 = V3 * V31; // + + V11 = _mm_shuffle_ps(NQ, Q, B3_SHUFFLE(2,3,1,3)); // -Z -W Y W + V11 = V11 * V21; // + V21 = _mm_xor_ps(V21, b3vMPPP); // change the sign of the first element + V31 = _mm_shuffle_ps(Q, NQ, B3_SHUFFLE(3,3,1,3)); // W W -Y -W + V31 = _mm_xor_ps(V31, b3vMPPP); // change the sign of the first element + Y = b3CastiTo128f(_mm_shuffle_epi32 (NQi, B3_SHUFFLE(3,2,0,3))); // -W -Z -X -W + Z = b3CastiTo128f(_mm_shuffle_epi32 (Qi, B3_SHUFFLE(1,0,1,3))); // Y X Y W + + vs = _mm_load_ss(&s); + V21 = V21 * Y; + V31 = V31 * Z; + + V1 = V1 + V11; + V2 = V2 + V21; + V3 = V3 + V31; + + vs = b3_splat3_ps(vs, 0); + // s ready + V1 = V1 * vs; + V2 = V2 * vs; + V3 = V3 * vs; + + V1 = V1 + b3v1000; + V2 = V2 + b3v0100; + V3 = V3 + b3v0010; + + m_el[0] = b3MakeVector3(V1); + m_el[1] = b3MakeVector3(V2); + m_el[2] = b3MakeVector3(V3); + #else + b3Scalar xs = q.getX() * s, ys = q.getY() * s, zs = q.getZ() * s; + b3Scalar wx = q.getW() * xs, wy = q.getW() * ys, wz = q.getW() * zs; + b3Scalar xx = q.getX() * xs, xy = q.getX() * ys, xz = q.getX() * zs; + b3Scalar yy = q.getY() * ys, yz = q.getY() * zs, zz = q.getZ() * zs; + setValue( + b3Scalar(1.0) - (yy + zz), xy - wz, xz + wy, + xy + wz, b3Scalar(1.0) - (xx + zz), yz - wx, + xz - wy, yz + wx, b3Scalar(1.0) - (xx + yy)); + #endif + } + + + /** @brief Set the matrix from euler angles using YPR around YXZ respectively + * @param yaw Yaw about Y axis + * @param pitch Pitch about X axis + * @param roll Roll about Z axis + */ + void setEulerYPR(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll) + { + setEulerZYX(roll, pitch, yaw); + } + + /** @brief Set the matrix from euler angles YPR around ZYX axes + * @param eulerX Roll about X axis + * @param eulerY Pitch around Y axis + * @param eulerZ Yaw aboud Z axis + * + * These angles are used to produce a rotation matrix. The euler + * angles are applied in ZYX order. I.e a vector is first rotated + * about X then Y and then Z + **/ + void setEulerZYX(b3Scalar eulerX,b3Scalar eulerY,b3Scalar eulerZ) { + ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code + b3Scalar ci ( b3Cos(eulerX)); + b3Scalar cj ( b3Cos(eulerY)); + b3Scalar ch ( b3Cos(eulerZ)); + b3Scalar si ( b3Sin(eulerX)); + b3Scalar sj ( b3Sin(eulerY)); + b3Scalar sh ( b3Sin(eulerZ)); + b3Scalar cc = ci * ch; + b3Scalar cs = ci * sh; + b3Scalar sc = si * ch; + b3Scalar ss = si * sh; + + setValue(cj * ch, sj * sc - cs, sj * cc + ss, + cj * sh, sj * ss + cc, sj * cs - sc, + -sj, cj * si, cj * ci); + } + + /**@brief Set the matrix to the identity */ + void setIdentity() + { +#if (defined(B3_USE_SSE_IN_API)&& defined (B3_USE_SSE)) || defined(B3_USE_NEON) + m_el[0] = b3MakeVector3(b3v1000); + m_el[1] = b3MakeVector3(b3v0100); + m_el[2] = b3MakeVector3(b3v0010); +#else + setValue(b3Scalar(1.0), b3Scalar(0.0), b3Scalar(0.0), + b3Scalar(0.0), b3Scalar(1.0), b3Scalar(0.0), + b3Scalar(0.0), b3Scalar(0.0), b3Scalar(1.0)); +#endif + } + + static const b3Matrix3x3& getIdentity() + { +#if (defined(B3_USE_SSE_IN_API)&& defined (B3_USE_SSE)) || defined(B3_USE_NEON) + static const b3Matrix3x3 + identityMatrix(b3v1000, b3v0100, b3v0010); +#else + static const b3Matrix3x3 + identityMatrix( + b3Scalar(1.0), b3Scalar(0.0), b3Scalar(0.0), + b3Scalar(0.0), b3Scalar(1.0), b3Scalar(0.0), + b3Scalar(0.0), b3Scalar(0.0), b3Scalar(1.0)); +#endif + return identityMatrix; + } + + /**@brief Fill the rotational part of an OpenGL matrix and clear the shear/perspective + * @param m The array to be filled */ + void getOpenGLSubMatrix(b3Scalar *m) const + { +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 v0 = m_el[0].mVec128; + __m128 v1 = m_el[1].mVec128; + __m128 v2 = m_el[2].mVec128; // x2 y2 z2 w2 + __m128 *vm = (__m128 *)m; + __m128 vT; + + v2 = _mm_and_ps(v2, b3vFFF0fMask); // x2 y2 z2 0 + + vT = _mm_unpackhi_ps(v0, v1); // z0 z1 * * + v0 = _mm_unpacklo_ps(v0, v1); // x0 x1 y0 y1 + + v1 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(2, 3, 1, 3) ); // y0 y1 y2 0 + v0 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(0, 1, 0, 3) ); // x0 x1 x2 0 + v2 = b3CastdTo128f(_mm_move_sd(b3CastfTo128d(v2), b3CastfTo128d(vT))); // z0 z1 z2 0 + + vm[0] = v0; + vm[1] = v1; + vm[2] = v2; +#elif defined(B3_USE_NEON) + // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions. + static const uint32x2_t zMask = (const uint32x2_t) {-1, 0 }; + float32x4_t *vm = (float32x4_t *)m; + float32x4x2_t top = vtrnq_f32( m_el[0].mVec128, m_el[1].mVec128 ); // {x0 x1 z0 z1}, {y0 y1 w0 w1} + float32x2x2_t bl = vtrn_f32( vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f) ); // {x2 0 }, {y2 0} + float32x4_t v0 = vcombine_f32( vget_low_f32(top.val[0]), bl.val[0] ); + float32x4_t v1 = vcombine_f32( vget_low_f32(top.val[1]), bl.val[1] ); + float32x2_t q = (float32x2_t) vand_u32( (uint32x2_t) vget_high_f32( m_el[2].mVec128), zMask ); + float32x4_t v2 = vcombine_f32( vget_high_f32(top.val[0]), q ); // z0 z1 z2 0 + + vm[0] = v0; + vm[1] = v1; + vm[2] = v2; +#else + m[0] = b3Scalar(m_el[0].getX()); + m[1] = b3Scalar(m_el[1].getX()); + m[2] = b3Scalar(m_el[2].getX()); + m[3] = b3Scalar(0.0); + m[4] = b3Scalar(m_el[0].getY()); + m[5] = b3Scalar(m_el[1].getY()); + m[6] = b3Scalar(m_el[2].getY()); + m[7] = b3Scalar(0.0); + m[8] = b3Scalar(m_el[0].getZ()); + m[9] = b3Scalar(m_el[1].getZ()); + m[10] = b3Scalar(m_el[2].getZ()); + m[11] = b3Scalar(0.0); +#endif + } + + /**@brief Get the matrix represented as a quaternion + * @param q The quaternion which will be set */ + void getRotation(b3Quaternion& q) const + { +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON) + b3Scalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ(); + b3Scalar s, x; + + union { + b3SimdFloat4 vec; + b3Scalar f[4]; + } temp; + + if (trace > b3Scalar(0.0)) + { + x = trace + b3Scalar(1.0); + + temp.f[0]=m_el[2].getY() - m_el[1].getZ(); + temp.f[1]=m_el[0].getZ() - m_el[2].getX(); + temp.f[2]=m_el[1].getX() - m_el[0].getY(); + temp.f[3]=x; + //temp.f[3]= s * b3Scalar(0.5); + } + else + { + int i, j, k; + if(m_el[0].getX() < m_el[1].getY()) + { + if( m_el[1].getY() < m_el[2].getZ() ) + { i = 2; j = 0; k = 1; } + else + { i = 1; j = 2; k = 0; } + } + else + { + if( m_el[0].getX() < m_el[2].getZ()) + { i = 2; j = 0; k = 1; } + else + { i = 0; j = 1; k = 2; } + } + + x = m_el[i][i] - m_el[j][j] - m_el[k][k] + b3Scalar(1.0); + + temp.f[3] = (m_el[k][j] - m_el[j][k]); + temp.f[j] = (m_el[j][i] + m_el[i][j]); + temp.f[k] = (m_el[k][i] + m_el[i][k]); + temp.f[i] = x; + //temp.f[i] = s * b3Scalar(0.5); + } + + s = b3Sqrt(x); + q.set128(temp.vec); + s = b3Scalar(0.5) / s; + + q *= s; +#else + b3Scalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ(); + + b3Scalar temp[4]; + + if (trace > b3Scalar(0.0)) + { + b3Scalar s = b3Sqrt(trace + b3Scalar(1.0)); + temp[3]=(s * b3Scalar(0.5)); + s = b3Scalar(0.5) / s; + + temp[0]=((m_el[2].getY() - m_el[1].getZ()) * s); + temp[1]=((m_el[0].getZ() - m_el[2].getX()) * s); + temp[2]=((m_el[1].getX() - m_el[0].getY()) * s); + } + else + { + int i = m_el[0].getX() < m_el[1].getY() ? + (m_el[1].getY() < m_el[2].getZ() ? 2 : 1) : + (m_el[0].getX() < m_el[2].getZ() ? 2 : 0); + int j = (i + 1) % 3; + int k = (i + 2) % 3; + + b3Scalar s = b3Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + b3Scalar(1.0)); + temp[i] = s * b3Scalar(0.5); + s = b3Scalar(0.5) / s; + + temp[3] = (m_el[k][j] - m_el[j][k]) * s; + temp[j] = (m_el[j][i] + m_el[i][j]) * s; + temp[k] = (m_el[k][i] + m_el[i][k]) * s; + } + q.setValue(temp[0],temp[1],temp[2],temp[3]); +#endif + } + + /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR + * @param yaw Yaw around Y axis + * @param pitch Pitch around X axis + * @param roll around Z axis */ + void getEulerYPR(b3Scalar& yaw, b3Scalar& pitch, b3Scalar& roll) const + { + + // first use the normal calculus + yaw = b3Scalar(b3Atan2(m_el[1].getX(), m_el[0].getX())); + pitch = b3Scalar(b3Asin(-m_el[2].getX())); + roll = b3Scalar(b3Atan2(m_el[2].getY(), m_el[2].getZ())); + + // on pitch = +/-HalfPI + if (b3Fabs(pitch)==B3_HALF_PI) + { + if (yaw>0) + yaw-=B3_PI; + else + yaw+=B3_PI; + + if (roll>0) + roll-=B3_PI; + else + roll+=B3_PI; + } + }; + + + /**@brief Get the matrix represented as euler angles around ZYX + * @param yaw Yaw around X axis + * @param pitch Pitch around Y axis + * @param roll around X axis + * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ + void getEulerZYX(b3Scalar& yaw, b3Scalar& pitch, b3Scalar& roll, unsigned int solution_number = 1) const + { + struct Euler + { + b3Scalar yaw; + b3Scalar pitch; + b3Scalar roll; + }; + + Euler euler_out; + Euler euler_out2; //second solution + //get the pointer to the raw data + + // Check that pitch is not at a singularity + if (b3Fabs(m_el[2].getX()) >= 1) + { + euler_out.yaw = 0; + euler_out2.yaw = 0; + + // From difference of angles formula + b3Scalar delta = b3Atan2(m_el[0].getX(),m_el[0].getZ()); + if (m_el[2].getX() > 0) //gimbal locked up + { + euler_out.pitch = B3_PI / b3Scalar(2.0); + euler_out2.pitch = B3_PI / b3Scalar(2.0); + euler_out.roll = euler_out.pitch + delta; + euler_out2.roll = euler_out.pitch + delta; + } + else // gimbal locked down + { + euler_out.pitch = -B3_PI / b3Scalar(2.0); + euler_out2.pitch = -B3_PI / b3Scalar(2.0); + euler_out.roll = -euler_out.pitch + delta; + euler_out2.roll = -euler_out.pitch + delta; + } + } + else + { + euler_out.pitch = - b3Asin(m_el[2].getX()); + euler_out2.pitch = B3_PI - euler_out.pitch; + + euler_out.roll = b3Atan2(m_el[2].getY()/b3Cos(euler_out.pitch), + m_el[2].getZ()/b3Cos(euler_out.pitch)); + euler_out2.roll = b3Atan2(m_el[2].getY()/b3Cos(euler_out2.pitch), + m_el[2].getZ()/b3Cos(euler_out2.pitch)); + + euler_out.yaw = b3Atan2(m_el[1].getX()/b3Cos(euler_out.pitch), + m_el[0].getX()/b3Cos(euler_out.pitch)); + euler_out2.yaw = b3Atan2(m_el[1].getX()/b3Cos(euler_out2.pitch), + m_el[0].getX()/b3Cos(euler_out2.pitch)); + } + + if (solution_number == 1) + { + yaw = euler_out.yaw; + pitch = euler_out.pitch; + roll = euler_out.roll; + } + else + { + yaw = euler_out2.yaw; + pitch = euler_out2.pitch; + roll = euler_out2.roll; + } + } + + /**@brief Create a scaled copy of the matrix + * @param s Scaling vector The elements of the vector will scale each column */ + + b3Matrix3x3 scaled(const b3Vector3& s) const + { +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON) + return b3Matrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s); +#else + return b3Matrix3x3( + m_el[0].getX() * s.getX(), m_el[0].getY() * s.getY(), m_el[0].getZ() * s.getZ(), + m_el[1].getX() * s.getX(), m_el[1].getY() * s.getY(), m_el[1].getZ() * s.getZ(), + m_el[2].getX() * s.getX(), m_el[2].getY() * s.getY(), m_el[2].getZ() * s.getZ()); +#endif + } + + /**@brief Return the determinant of the matrix */ + b3Scalar determinant() const; + /**@brief Return the adjoint of the matrix */ + b3Matrix3x3 adjoint() const; + /**@brief Return the matrix with all values non negative */ + b3Matrix3x3 absolute() const; + /**@brief Return the transpose of the matrix */ + b3Matrix3x3 transpose() const; + /**@brief Return the inverse of the matrix */ + b3Matrix3x3 inverse() const; + + b3Matrix3x3 transposeTimes(const b3Matrix3x3& m) const; + b3Matrix3x3 timesTranspose(const b3Matrix3x3& m) const; + + B3_FORCE_INLINE b3Scalar tdotx(const b3Vector3& v) const + { + return m_el[0].getX() * v.getX() + m_el[1].getX() * v.getY() + m_el[2].getX() * v.getZ(); + } + B3_FORCE_INLINE b3Scalar tdoty(const b3Vector3& v) const + { + return m_el[0].getY() * v.getX() + m_el[1].getY() * v.getY() + m_el[2].getY() * v.getZ(); + } + B3_FORCE_INLINE b3Scalar tdotz(const b3Vector3& v) const + { + return m_el[0].getZ() * v.getX() + m_el[1].getZ() * v.getY() + m_el[2].getZ() * v.getZ(); + } + + + /**@brief diagonalizes this matrix by the Jacobi method. + * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original + * coordinate system, i.e., old_this = rot * new_this * rot^T. + * @param threshold See iteration + * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied + * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. + * + * Note that this matrix is assumed to be symmetric. + */ + void diagonalize(b3Matrix3x3& rot, b3Scalar threshold, int maxSteps) + { + rot.setIdentity(); + for (int step = maxSteps; step > 0; step--) + { + // find off-diagonal element [p][q] with largest magnitude + int p = 0; + int q = 1; + int r = 2; + b3Scalar max = b3Fabs(m_el[0][1]); + b3Scalar v = b3Fabs(m_el[0][2]); + if (v > max) + { + q = 2; + r = 1; + max = v; + } + v = b3Fabs(m_el[1][2]); + if (v > max) + { + p = 1; + q = 2; + r = 0; + max = v; + } + + b3Scalar t = threshold * (b3Fabs(m_el[0][0]) + b3Fabs(m_el[1][1]) + b3Fabs(m_el[2][2])); + if (max <= t) + { + if (max <= B3_EPSILON * t) + { + return; + } + step = 1; + } + + // compute Jacobi rotation J which leads to a zero for element [p][q] + b3Scalar mpq = m_el[p][q]; + b3Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq); + b3Scalar theta2 = theta * theta; + b3Scalar cos; + b3Scalar sin; + if (theta2 * theta2 < b3Scalar(10 / B3_EPSILON)) + { + t = (theta >= 0) ? 1 / (theta + b3Sqrt(1 + theta2)) + : 1 / (theta - b3Sqrt(1 + theta2)); + cos = 1 / b3Sqrt(1 + t * t); + sin = cos * t; + } + else + { + // approximation for large theta-value, i.e., a nearly diagonal matrix + t = 1 / (theta * (2 + b3Scalar(0.5) / theta2)); + cos = 1 - b3Scalar(0.5) * t * t; + sin = cos * t; + } + + // apply rotation to matrix (this = J^T * this * J) + m_el[p][q] = m_el[q][p] = 0; + m_el[p][p] -= t * mpq; + m_el[q][q] += t * mpq; + b3Scalar mrp = m_el[r][p]; + b3Scalar mrq = m_el[r][q]; + m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq; + m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp; + + // apply rotation to rot (rot = rot * J) + for (int i = 0; i < 3; i++) + { + b3Vector3& row = rot[i]; + mrp = row[p]; + mrq = row[q]; + row[p] = cos * mrp - sin * mrq; + row[q] = cos * mrq + sin * mrp; + } + } + } + + + + + /**@brief Calculate the matrix cofactor + * @param r1 The first row to use for calculating the cofactor + * @param c1 The first column to use for calculating the cofactor + * @param r1 The second row to use for calculating the cofactor + * @param c1 The second column to use for calculating the cofactor + * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details + */ + b3Scalar cofac(int r1, int c1, int r2, int c2) const + { + return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; + } + + void serialize(struct b3Matrix3x3Data& dataOut) const; + + void serializeFloat(struct b3Matrix3x3FloatData& dataOut) const; + + void deSerialize(const struct b3Matrix3x3Data& dataIn); + + void deSerializeFloat(const struct b3Matrix3x3FloatData& dataIn); + + void deSerializeDouble(const struct b3Matrix3x3DoubleData& dataIn); + +}; + + +B3_FORCE_INLINE b3Matrix3x3& +b3Matrix3x3::operator*=(const b3Matrix3x3& m) +{ +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 rv00, rv01, rv02; + __m128 rv10, rv11, rv12; + __m128 rv20, rv21, rv22; + __m128 mv0, mv1, mv2; + + rv02 = m_el[0].mVec128; + rv12 = m_el[1].mVec128; + rv22 = m_el[2].mVec128; + + mv0 = _mm_and_ps(m[0].mVec128, b3vFFF0fMask); + mv1 = _mm_and_ps(m[1].mVec128, b3vFFF0fMask); + mv2 = _mm_and_ps(m[2].mVec128, b3vFFF0fMask); + + // rv0 + rv00 = b3_splat_ps(rv02, 0); + rv01 = b3_splat_ps(rv02, 1); + rv02 = b3_splat_ps(rv02, 2); + + rv00 = _mm_mul_ps(rv00, mv0); + rv01 = _mm_mul_ps(rv01, mv1); + rv02 = _mm_mul_ps(rv02, mv2); + + // rv1 + rv10 = b3_splat_ps(rv12, 0); + rv11 = b3_splat_ps(rv12, 1); + rv12 = b3_splat_ps(rv12, 2); + + rv10 = _mm_mul_ps(rv10, mv0); + rv11 = _mm_mul_ps(rv11, mv1); + rv12 = _mm_mul_ps(rv12, mv2); + + // rv2 + rv20 = b3_splat_ps(rv22, 0); + rv21 = b3_splat_ps(rv22, 1); + rv22 = b3_splat_ps(rv22, 2); + + rv20 = _mm_mul_ps(rv20, mv0); + rv21 = _mm_mul_ps(rv21, mv1); + rv22 = _mm_mul_ps(rv22, mv2); + + rv00 = _mm_add_ps(rv00, rv01); + rv10 = _mm_add_ps(rv10, rv11); + rv20 = _mm_add_ps(rv20, rv21); + + m_el[0].mVec128 = _mm_add_ps(rv00, rv02); + m_el[1].mVec128 = _mm_add_ps(rv10, rv12); + m_el[2].mVec128 = _mm_add_ps(rv20, rv22); + +#elif defined(B3_USE_NEON) + + float32x4_t rv0, rv1, rv2; + float32x4_t v0, v1, v2; + float32x4_t mv0, mv1, mv2; + + v0 = m_el[0].mVec128; + v1 = m_el[1].mVec128; + v2 = m_el[2].mVec128; + + mv0 = (float32x4_t) vandq_s32((int32x4_t)m[0].mVec128, b3vFFF0Mask); + mv1 = (float32x4_t) vandq_s32((int32x4_t)m[1].mVec128, b3vFFF0Mask); + mv2 = (float32x4_t) vandq_s32((int32x4_t)m[2].mVec128, b3vFFF0Mask); + + rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0); + rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0); + rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0); + + rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1); + rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1); + rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1); + + rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0); + rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0); + rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0); + + m_el[0].mVec128 = rv0; + m_el[1].mVec128 = rv1; + m_el[2].mVec128 = rv2; +#else + setValue( + m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]), + m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]), + m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2])); +#endif + return *this; +} + +B3_FORCE_INLINE b3Matrix3x3& +b3Matrix3x3::operator+=(const b3Matrix3x3& m) +{ +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON) + m_el[0].mVec128 = m_el[0].mVec128 + m.m_el[0].mVec128; + m_el[1].mVec128 = m_el[1].mVec128 + m.m_el[1].mVec128; + m_el[2].mVec128 = m_el[2].mVec128 + m.m_el[2].mVec128; +#else + setValue( + m_el[0][0]+m.m_el[0][0], + m_el[0][1]+m.m_el[0][1], + m_el[0][2]+m.m_el[0][2], + m_el[1][0]+m.m_el[1][0], + m_el[1][1]+m.m_el[1][1], + m_el[1][2]+m.m_el[1][2], + m_el[2][0]+m.m_el[2][0], + m_el[2][1]+m.m_el[2][1], + m_el[2][2]+m.m_el[2][2]); +#endif + return *this; +} + +B3_FORCE_INLINE b3Matrix3x3 +operator*(const b3Matrix3x3& m, const b3Scalar & k) +{ +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)) + __m128 vk = b3_splat_ps(_mm_load_ss((float *)&k), 0x80); + return b3Matrix3x3( + _mm_mul_ps(m[0].mVec128, vk), + _mm_mul_ps(m[1].mVec128, vk), + _mm_mul_ps(m[2].mVec128, vk)); +#elif defined(B3_USE_NEON) + return b3Matrix3x3( + vmulq_n_f32(m[0].mVec128, k), + vmulq_n_f32(m[1].mVec128, k), + vmulq_n_f32(m[2].mVec128, k)); +#else + return b3Matrix3x3( + m[0].getX()*k,m[0].getY()*k,m[0].getZ()*k, + m[1].getX()*k,m[1].getY()*k,m[1].getZ()*k, + m[2].getX()*k,m[2].getY()*k,m[2].getZ()*k); +#endif +} + +B3_FORCE_INLINE b3Matrix3x3 +operator+(const b3Matrix3x3& m1, const b3Matrix3x3& m2) +{ +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON) + return b3Matrix3x3( + m1[0].mVec128 + m2[0].mVec128, + m1[1].mVec128 + m2[1].mVec128, + m1[2].mVec128 + m2[2].mVec128); +#else + return b3Matrix3x3( + m1[0][0]+m2[0][0], + m1[0][1]+m2[0][1], + m1[0][2]+m2[0][2], + + m1[1][0]+m2[1][0], + m1[1][1]+m2[1][1], + m1[1][2]+m2[1][2], + + m1[2][0]+m2[2][0], + m1[2][1]+m2[2][1], + m1[2][2]+m2[2][2]); +#endif +} + +B3_FORCE_INLINE b3Matrix3x3 +operator-(const b3Matrix3x3& m1, const b3Matrix3x3& m2) +{ +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON) + return b3Matrix3x3( + m1[0].mVec128 - m2[0].mVec128, + m1[1].mVec128 - m2[1].mVec128, + m1[2].mVec128 - m2[2].mVec128); +#else + return b3Matrix3x3( + m1[0][0]-m2[0][0], + m1[0][1]-m2[0][1], + m1[0][2]-m2[0][2], + + m1[1][0]-m2[1][0], + m1[1][1]-m2[1][1], + m1[1][2]-m2[1][2], + + m1[2][0]-m2[2][0], + m1[2][1]-m2[2][1], + m1[2][2]-m2[2][2]); +#endif +} + + +B3_FORCE_INLINE b3Matrix3x3& +b3Matrix3x3::operator-=(const b3Matrix3x3& m) +{ +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON) + m_el[0].mVec128 = m_el[0].mVec128 - m.m_el[0].mVec128; + m_el[1].mVec128 = m_el[1].mVec128 - m.m_el[1].mVec128; + m_el[2].mVec128 = m_el[2].mVec128 - m.m_el[2].mVec128; +#else + setValue( + m_el[0][0]-m.m_el[0][0], + m_el[0][1]-m.m_el[0][1], + m_el[0][2]-m.m_el[0][2], + m_el[1][0]-m.m_el[1][0], + m_el[1][1]-m.m_el[1][1], + m_el[1][2]-m.m_el[1][2], + m_el[2][0]-m.m_el[2][0], + m_el[2][1]-m.m_el[2][1], + m_el[2][2]-m.m_el[2][2]); +#endif + return *this; +} + + +B3_FORCE_INLINE b3Scalar +b3Matrix3x3::determinant() const +{ + return b3Triple((*this)[0], (*this)[1], (*this)[2]); +} + + +B3_FORCE_INLINE b3Matrix3x3 +b3Matrix3x3::absolute() const +{ +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)) + return b3Matrix3x3( + _mm_and_ps(m_el[0].mVec128, b3vAbsfMask), + _mm_and_ps(m_el[1].mVec128, b3vAbsfMask), + _mm_and_ps(m_el[2].mVec128, b3vAbsfMask)); +#elif defined(B3_USE_NEON) + return b3Matrix3x3( + (float32x4_t)vandq_s32((int32x4_t)m_el[0].mVec128, b3v3AbsMask), + (float32x4_t)vandq_s32((int32x4_t)m_el[1].mVec128, b3v3AbsMask), + (float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, b3v3AbsMask)); +#else + return b3Matrix3x3( + b3Fabs(m_el[0].getX()), b3Fabs(m_el[0].getY()), b3Fabs(m_el[0].getZ()), + b3Fabs(m_el[1].getX()), b3Fabs(m_el[1].getY()), b3Fabs(m_el[1].getZ()), + b3Fabs(m_el[2].getX()), b3Fabs(m_el[2].getY()), b3Fabs(m_el[2].getZ())); +#endif +} + +B3_FORCE_INLINE b3Matrix3x3 +b3Matrix3x3::transpose() const +{ +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)) + __m128 v0 = m_el[0].mVec128; + __m128 v1 = m_el[1].mVec128; + __m128 v2 = m_el[2].mVec128; // x2 y2 z2 w2 + __m128 vT; + + v2 = _mm_and_ps(v2, b3vFFF0fMask); // x2 y2 z2 0 + + vT = _mm_unpackhi_ps(v0, v1); // z0 z1 * * + v0 = _mm_unpacklo_ps(v0, v1); // x0 x1 y0 y1 + + v1 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(2, 3, 1, 3) ); // y0 y1 y2 0 + v0 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(0, 1, 0, 3) ); // x0 x1 x2 0 + v2 = b3CastdTo128f(_mm_move_sd(b3CastfTo128d(v2), b3CastfTo128d(vT))); // z0 z1 z2 0 + + + return b3Matrix3x3( v0, v1, v2 ); +#elif defined(B3_USE_NEON) + // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions. + static const uint32x2_t zMask = (const uint32x2_t) {-1, 0 }; + float32x4x2_t top = vtrnq_f32( m_el[0].mVec128, m_el[1].mVec128 ); // {x0 x1 z0 z1}, {y0 y1 w0 w1} + float32x2x2_t bl = vtrn_f32( vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f) ); // {x2 0 }, {y2 0} + float32x4_t v0 = vcombine_f32( vget_low_f32(top.val[0]), bl.val[0] ); + float32x4_t v1 = vcombine_f32( vget_low_f32(top.val[1]), bl.val[1] ); + float32x2_t q = (float32x2_t) vand_u32( (uint32x2_t) vget_high_f32( m_el[2].mVec128), zMask ); + float32x4_t v2 = vcombine_f32( vget_high_f32(top.val[0]), q ); // z0 z1 z2 0 + return b3Matrix3x3( v0, v1, v2 ); +#else + return b3Matrix3x3( m_el[0].getX(), m_el[1].getX(), m_el[2].getX(), + m_el[0].getY(), m_el[1].getY(), m_el[2].getY(), + m_el[0].getZ(), m_el[1].getZ(), m_el[2].getZ()); +#endif +} + +B3_FORCE_INLINE b3Matrix3x3 +b3Matrix3x3::adjoint() const +{ + return b3Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2), + cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0), + cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1)); +} + +B3_FORCE_INLINE b3Matrix3x3 +b3Matrix3x3::inverse() const +{ + b3Vector3 co = b3MakeVector3(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); + b3Scalar det = (*this)[0].dot(co); + b3FullAssert(det != b3Scalar(0.0)); + b3Scalar s = b3Scalar(1.0) / det; + return b3Matrix3x3(co.getX() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, + co.getY() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, + co.getZ() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); +} + +B3_FORCE_INLINE b3Matrix3x3 +b3Matrix3x3::transposeTimes(const b3Matrix3x3& m) const +{ +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)) + // zeros w +// static const __m128i xyzMask = (const __m128i){ -1ULL, 0xffffffffULL }; + __m128 row = m_el[0].mVec128; + __m128 m0 = _mm_and_ps( m.getRow(0).mVec128, b3vFFF0fMask ); + __m128 m1 = _mm_and_ps( m.getRow(1).mVec128, b3vFFF0fMask); + __m128 m2 = _mm_and_ps( m.getRow(2).mVec128, b3vFFF0fMask ); + __m128 r0 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0)); + __m128 r1 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0x55)); + __m128 r2 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0xaa)); + row = m_el[1].mVec128; + r0 = _mm_add_ps( r0, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0))); + r1 = _mm_add_ps( r1, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0x55))); + r2 = _mm_add_ps( r2, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0xaa))); + row = m_el[2].mVec128; + r0 = _mm_add_ps( r0, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0))); + r1 = _mm_add_ps( r1, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0x55))); + r2 = _mm_add_ps( r2, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0xaa))); + return b3Matrix3x3( r0, r1, r2 ); + +#elif defined B3_USE_NEON + // zeros w + static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 }; + float32x4_t m0 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(0).mVec128, xyzMask ); + float32x4_t m1 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(1).mVec128, xyzMask ); + float32x4_t m2 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(2).mVec128, xyzMask ); + float32x4_t row = m_el[0].mVec128; + float32x4_t r0 = vmulq_lane_f32( m0, vget_low_f32(row), 0); + float32x4_t r1 = vmulq_lane_f32( m0, vget_low_f32(row), 1); + float32x4_t r2 = vmulq_lane_f32( m0, vget_high_f32(row), 0); + row = m_el[1].mVec128; + r0 = vmlaq_lane_f32( r0, m1, vget_low_f32(row), 0); + r1 = vmlaq_lane_f32( r1, m1, vget_low_f32(row), 1); + r2 = vmlaq_lane_f32( r2, m1, vget_high_f32(row), 0); + row = m_el[2].mVec128; + r0 = vmlaq_lane_f32( r0, m2, vget_low_f32(row), 0); + r1 = vmlaq_lane_f32( r1, m2, vget_low_f32(row), 1); + r2 = vmlaq_lane_f32( r2, m2, vget_high_f32(row), 0); + return b3Matrix3x3( r0, r1, r2 ); +#else + return b3Matrix3x3( + m_el[0].getX() * m[0].getX() + m_el[1].getX() * m[1].getX() + m_el[2].getX() * m[2].getX(), + m_el[0].getX() * m[0].getY() + m_el[1].getX() * m[1].getY() + m_el[2].getX() * m[2].getY(), + m_el[0].getX() * m[0].getZ() + m_el[1].getX() * m[1].getZ() + m_el[2].getX() * m[2].getZ(), + m_el[0].getY() * m[0].getX() + m_el[1].getY() * m[1].getX() + m_el[2].getY() * m[2].getX(), + m_el[0].getY() * m[0].getY() + m_el[1].getY() * m[1].getY() + m_el[2].getY() * m[2].getY(), + m_el[0].getY() * m[0].getZ() + m_el[1].getY() * m[1].getZ() + m_el[2].getY() * m[2].getZ(), + m_el[0].getZ() * m[0].getX() + m_el[1].getZ() * m[1].getX() + m_el[2].getZ() * m[2].getX(), + m_el[0].getZ() * m[0].getY() + m_el[1].getZ() * m[1].getY() + m_el[2].getZ() * m[2].getY(), + m_el[0].getZ() * m[0].getZ() + m_el[1].getZ() * m[1].getZ() + m_el[2].getZ() * m[2].getZ()); +#endif +} + +B3_FORCE_INLINE b3Matrix3x3 +b3Matrix3x3::timesTranspose(const b3Matrix3x3& m) const +{ +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)) + __m128 a0 = m_el[0].mVec128; + __m128 a1 = m_el[1].mVec128; + __m128 a2 = m_el[2].mVec128; + + b3Matrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here + __m128 mx = mT[0].mVec128; + __m128 my = mT[1].mVec128; + __m128 mz = mT[2].mVec128; + + __m128 r0 = _mm_mul_ps(mx, _mm_shuffle_ps(a0, a0, 0x00)); + __m128 r1 = _mm_mul_ps(mx, _mm_shuffle_ps(a1, a1, 0x00)); + __m128 r2 = _mm_mul_ps(mx, _mm_shuffle_ps(a2, a2, 0x00)); + r0 = _mm_add_ps(r0, _mm_mul_ps(my, _mm_shuffle_ps(a0, a0, 0x55))); + r1 = _mm_add_ps(r1, _mm_mul_ps(my, _mm_shuffle_ps(a1, a1, 0x55))); + r2 = _mm_add_ps(r2, _mm_mul_ps(my, _mm_shuffle_ps(a2, a2, 0x55))); + r0 = _mm_add_ps(r0, _mm_mul_ps(mz, _mm_shuffle_ps(a0, a0, 0xaa))); + r1 = _mm_add_ps(r1, _mm_mul_ps(mz, _mm_shuffle_ps(a1, a1, 0xaa))); + r2 = _mm_add_ps(r2, _mm_mul_ps(mz, _mm_shuffle_ps(a2, a2, 0xaa))); + return b3Matrix3x3( r0, r1, r2); + +#elif defined B3_USE_NEON + float32x4_t a0 = m_el[0].mVec128; + float32x4_t a1 = m_el[1].mVec128; + float32x4_t a2 = m_el[2].mVec128; + + b3Matrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here + float32x4_t mx = mT[0].mVec128; + float32x4_t my = mT[1].mVec128; + float32x4_t mz = mT[2].mVec128; + + float32x4_t r0 = vmulq_lane_f32( mx, vget_low_f32(a0), 0); + float32x4_t r1 = vmulq_lane_f32( mx, vget_low_f32(a1), 0); + float32x4_t r2 = vmulq_lane_f32( mx, vget_low_f32(a2), 0); + r0 = vmlaq_lane_f32( r0, my, vget_low_f32(a0), 1); + r1 = vmlaq_lane_f32( r1, my, vget_low_f32(a1), 1); + r2 = vmlaq_lane_f32( r2, my, vget_low_f32(a2), 1); + r0 = vmlaq_lane_f32( r0, mz, vget_high_f32(a0), 0); + r1 = vmlaq_lane_f32( r1, mz, vget_high_f32(a1), 0); + r2 = vmlaq_lane_f32( r2, mz, vget_high_f32(a2), 0); + return b3Matrix3x3( r0, r1, r2 ); + +#else + return b3Matrix3x3( + m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]), + m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]), + m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2])); +#endif +} + +B3_FORCE_INLINE b3Vector3 +operator*(const b3Matrix3x3& m, const b3Vector3& v) +{ +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON) + return v.dot3(m[0], m[1], m[2]); +#else + return b3MakeVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); +#endif +} + + +B3_FORCE_INLINE b3Vector3 +operator*(const b3Vector3& v, const b3Matrix3x3& m) +{ +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)) + + const __m128 vv = v.mVec128; + + __m128 c0 = b3_splat_ps( vv, 0); + __m128 c1 = b3_splat_ps( vv, 1); + __m128 c2 = b3_splat_ps( vv, 2); + + c0 = _mm_mul_ps(c0, _mm_and_ps(m[0].mVec128, b3vFFF0fMask) ); + c1 = _mm_mul_ps(c1, _mm_and_ps(m[1].mVec128, b3vFFF0fMask) ); + c0 = _mm_add_ps(c0, c1); + c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, b3vFFF0fMask) ); + + return b3MakeVector3(_mm_add_ps(c0, c2)); +#elif defined(B3_USE_NEON) + const float32x4_t vv = v.mVec128; + const float32x2_t vlo = vget_low_f32(vv); + const float32x2_t vhi = vget_high_f32(vv); + + float32x4_t c0, c1, c2; + + c0 = (float32x4_t) vandq_s32((int32x4_t)m[0].mVec128, b3vFFF0Mask); + c1 = (float32x4_t) vandq_s32((int32x4_t)m[1].mVec128, b3vFFF0Mask); + c2 = (float32x4_t) vandq_s32((int32x4_t)m[2].mVec128, b3vFFF0Mask); + + c0 = vmulq_lane_f32(c0, vlo, 0); + c1 = vmulq_lane_f32(c1, vlo, 1); + c2 = vmulq_lane_f32(c2, vhi, 0); + c0 = vaddq_f32(c0, c1); + c0 = vaddq_f32(c0, c2); + + return b3MakeVector3(c0); +#else + return b3MakeVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v)); +#endif +} + +B3_FORCE_INLINE b3Matrix3x3 +operator*(const b3Matrix3x3& m1, const b3Matrix3x3& m2) +{ +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)) + + __m128 m10 = m1[0].mVec128; + __m128 m11 = m1[1].mVec128; + __m128 m12 = m1[2].mVec128; + + __m128 m2v = _mm_and_ps(m2[0].mVec128, b3vFFF0fMask); + + __m128 c0 = b3_splat_ps( m10, 0); + __m128 c1 = b3_splat_ps( m11, 0); + __m128 c2 = b3_splat_ps( m12, 0); + + c0 = _mm_mul_ps(c0, m2v); + c1 = _mm_mul_ps(c1, m2v); + c2 = _mm_mul_ps(c2, m2v); + + m2v = _mm_and_ps(m2[1].mVec128, b3vFFF0fMask); + + __m128 c0_1 = b3_splat_ps( m10, 1); + __m128 c1_1 = b3_splat_ps( m11, 1); + __m128 c2_1 = b3_splat_ps( m12, 1); + + c0_1 = _mm_mul_ps(c0_1, m2v); + c1_1 = _mm_mul_ps(c1_1, m2v); + c2_1 = _mm_mul_ps(c2_1, m2v); + + m2v = _mm_and_ps(m2[2].mVec128, b3vFFF0fMask); + + c0 = _mm_add_ps(c0, c0_1); + c1 = _mm_add_ps(c1, c1_1); + c2 = _mm_add_ps(c2, c2_1); + + m10 = b3_splat_ps( m10, 2); + m11 = b3_splat_ps( m11, 2); + m12 = b3_splat_ps( m12, 2); + + m10 = _mm_mul_ps(m10, m2v); + m11 = _mm_mul_ps(m11, m2v); + m12 = _mm_mul_ps(m12, m2v); + + c0 = _mm_add_ps(c0, m10); + c1 = _mm_add_ps(c1, m11); + c2 = _mm_add_ps(c2, m12); + + return b3Matrix3x3(c0, c1, c2); + +#elif defined(B3_USE_NEON) + + float32x4_t rv0, rv1, rv2; + float32x4_t v0, v1, v2; + float32x4_t mv0, mv1, mv2; + + v0 = m1[0].mVec128; + v1 = m1[1].mVec128; + v2 = m1[2].mVec128; + + mv0 = (float32x4_t) vandq_s32((int32x4_t)m2[0].mVec128, b3vFFF0Mask); + mv1 = (float32x4_t) vandq_s32((int32x4_t)m2[1].mVec128, b3vFFF0Mask); + mv2 = (float32x4_t) vandq_s32((int32x4_t)m2[2].mVec128, b3vFFF0Mask); + + rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0); + rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0); + rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0); + + rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1); + rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1); + rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1); + + rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0); + rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0); + rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0); + + return b3Matrix3x3(rv0, rv1, rv2); + +#else + return b3Matrix3x3( + m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]), + m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]), + m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2])); +#endif +} + +/* +B3_FORCE_INLINE b3Matrix3x3 b3MultTransposeLeft(const b3Matrix3x3& m1, const b3Matrix3x3& m2) { +return b3Matrix3x3( +m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], +m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1], +m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2], +m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], +m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], +m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], +m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], +m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], +m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); +} +*/ + +/**@brief Equality operator between two matrices +* It will test all elements are equal. */ +B3_FORCE_INLINE bool operator==(const b3Matrix3x3& m1, const b3Matrix3x3& m2) +{ +#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)) + + __m128 c0, c1, c2; + + c0 = _mm_cmpeq_ps(m1[0].mVec128, m2[0].mVec128); + c1 = _mm_cmpeq_ps(m1[1].mVec128, m2[1].mVec128); + c2 = _mm_cmpeq_ps(m1[2].mVec128, m2[2].mVec128); + + c0 = _mm_and_ps(c0, c1); + c0 = _mm_and_ps(c0, c2); + + return (0x7 == _mm_movemask_ps((__m128)c0)); +#else + return + ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && + m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] && + m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] ); +#endif +} + +///for serialization +struct b3Matrix3x3FloatData +{ + b3Vector3FloatData m_el[3]; +}; + +///for serialization +struct b3Matrix3x3DoubleData +{ + b3Vector3DoubleData m_el[3]; +}; + + + + +B3_FORCE_INLINE void b3Matrix3x3::serialize(struct b3Matrix3x3Data& dataOut) const +{ + for (int i=0;i<3;i++) + m_el[i].serialize(dataOut.m_el[i]); +} + +B3_FORCE_INLINE void b3Matrix3x3::serializeFloat(struct b3Matrix3x3FloatData& dataOut) const +{ + for (int i=0;i<3;i++) + m_el[i].serializeFloat(dataOut.m_el[i]); +} + + +B3_FORCE_INLINE void b3Matrix3x3::deSerialize(const struct b3Matrix3x3Data& dataIn) +{ + for (int i=0;i<3;i++) + m_el[i].deSerialize(dataIn.m_el[i]); +} + +B3_FORCE_INLINE void b3Matrix3x3::deSerializeFloat(const struct b3Matrix3x3FloatData& dataIn) +{ + for (int i=0;i<3;i++) + m_el[i].deSerializeFloat(dataIn.m_el[i]); +} + +B3_FORCE_INLINE void b3Matrix3x3::deSerializeDouble(const struct b3Matrix3x3DoubleData& dataIn) +{ + for (int i=0;i<3;i++) + m_el[i].deSerializeDouble(dataIn.m_el[i]); +} + +#endif //B3_MATRIX3x3_H + diff --git a/extern/bullet/src/Bullet3Common/b3MinMax.h b/extern/bullet/src/Bullet3Common/b3MinMax.h new file mode 100644 index 000000000000..73af23a4f97d --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3MinMax.h @@ -0,0 +1,71 @@ +/* +Copyright (c) 2003-2013 Gino van den Bergen / 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. +*/ + + + +#ifndef B3_GEN_MINMAX_H +#define B3_GEN_MINMAX_H + +#include "b3Scalar.h" + +template +B3_FORCE_INLINE const T& b3Min(const T& a, const T& b) +{ + return a < b ? a : b ; +} + +template +B3_FORCE_INLINE const T& b3Max(const T& a, const T& b) +{ + return a > b ? a : b; +} + +template +B3_FORCE_INLINE const T& b3Clamped(const T& a, const T& lb, const T& ub) +{ + return a < lb ? lb : (ub < a ? ub : a); +} + +template +B3_FORCE_INLINE void b3SetMin(T& a, const T& b) +{ + if (b < a) + { + a = b; + } +} + +template +B3_FORCE_INLINE void b3SetMax(T& a, const T& b) +{ + if (a < b) + { + a = b; + } +} + +template +B3_FORCE_INLINE void b3Clamp(T& a, const T& lb, const T& ub) +{ + if (a < lb) + { + a = lb; + } + else if (ub < a) + { + a = ub; + } +} + +#endif //B3_GEN_MINMAX_H diff --git a/extern/bullet/src/Bullet3Common/b3PoolAllocator.h b/extern/bullet/src/Bullet3Common/b3PoolAllocator.h new file mode 100644 index 000000000000..2fcdcf5b2491 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3PoolAllocator.h @@ -0,0 +1,121 @@ +/* +Copyright (c) 2003-2013 Gino van den Bergen / 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. +*/ + + +#ifndef _BT_POOL_ALLOCATOR_H +#define _BT_POOL_ALLOCATOR_H + +#include "b3Scalar.h" +#include "b3AlignedAllocator.h" + +///The b3PoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately. +class b3PoolAllocator +{ + int m_elemSize; + int m_maxElements; + int m_freeCount; + void* m_firstFree; + unsigned char* m_pool; + +public: + + b3PoolAllocator(int elemSize, int maxElements) + :m_elemSize(elemSize), + m_maxElements(maxElements) + { + m_pool = (unsigned char*) b3AlignedAlloc( static_cast(m_elemSize*m_maxElements),16); + + unsigned char* p = m_pool; + m_firstFree = p; + m_freeCount = m_maxElements; + int count = m_maxElements; + while (--count) { + *(void**)p = (p + m_elemSize); + p += m_elemSize; + } + *(void**)p = 0; + } + + ~b3PoolAllocator() + { + b3AlignedFree( m_pool); + } + + int getFreeCount() const + { + return m_freeCount; + } + + int getUsedCount() const + { + return m_maxElements - m_freeCount; + } + + int getMaxCount() const + { + return m_maxElements; + } + + void* allocate(int size) + { + // release mode fix + (void)size; + b3Assert(!size || size<=m_elemSize); + b3Assert(m_freeCount>0); + void* result = m_firstFree; + m_firstFree = *(void**)m_firstFree; + --m_freeCount; + return result; + } + + bool validPtr(void* ptr) + { + if (ptr) { + if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize)) + { + return true; + } + } + return false; + } + + void freeMemory(void* ptr) + { + if (ptr) { + b3Assert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize); + + *(void**)ptr = m_firstFree; + m_firstFree = ptr; + ++m_freeCount; + } + } + + int getElementSize() const + { + return m_elemSize; + } + + unsigned char* getPoolAddress() + { + return m_pool; + } + + const unsigned char* getPoolAddress() const + { + return m_pool; + } + +}; + +#endif //_BT_POOL_ALLOCATOR_H diff --git a/extern/bullet/src/Bullet3Common/b3QuadWord.h b/extern/bullet/src/Bullet3Common/b3QuadWord.h new file mode 100644 index 000000000000..65c95819775c --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3QuadWord.h @@ -0,0 +1,245 @@ +/* +Copyright (c) 2003-2013 Gino van den Bergen / 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. +*/ + + +#ifndef B3_SIMD_QUADWORD_H +#define B3_SIMD_QUADWORD_H + +#include "b3Scalar.h" +#include "b3MinMax.h" + + + + + +#if defined (__CELLOS_LV2) && defined (__SPU__) +#include +#endif + +/**@brief The b3QuadWord class is base class for b3Vector3 and b3Quaternion. + * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. + */ +#ifndef USE_LIBSPE2 +B3_ATTRIBUTE_ALIGNED16(class) b3QuadWord +#else +class b3QuadWord +#endif +{ +protected: + +#if defined (__SPU__) && defined (__CELLOS_LV2__) + union { + vec_float4 mVec128; + b3Scalar m_floats[4]; + }; +public: + vec_float4 get128() const + { + return mVec128; + } + +#else //__CELLOS_LV2__ __SPU__ + +#if defined(B3_USE_SSE) || defined(B3_USE_NEON) +public: + union { + b3SimdFloat4 mVec128; + b3Scalar m_floats[4]; + struct {b3Scalar x,y,z,w;}; + }; +public: + B3_FORCE_INLINE b3SimdFloat4 get128() const + { + return mVec128; + } + B3_FORCE_INLINE void set128(b3SimdFloat4 v128) + { + mVec128 = v128; + } +#else +public: + union + { + b3Scalar m_floats[4]; + struct {b3Scalar x,y,z,w;}; + }; +#endif // B3_USE_SSE + +#endif //__CELLOS_LV2__ __SPU__ + + public: + +#if defined(B3_USE_SSE) || defined(B3_USE_NEON) + + // Set Vector + B3_FORCE_INLINE b3QuadWord(const b3SimdFloat4 vec) + { + mVec128 = vec; + } + + // Copy constructor + B3_FORCE_INLINE b3QuadWord(const b3QuadWord& rhs) + { + mVec128 = rhs.mVec128; + } + + // Assignment Operator + B3_FORCE_INLINE b3QuadWord& + operator=(const b3QuadWord& v) + { + mVec128 = v.mVec128; + + return *this; + } + +#endif + + /**@brief Return the x value */ + B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; } + /**@brief Return the y value */ + B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; } + /**@brief Return the z value */ + B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; } + /**@brief Set the x value */ + B3_FORCE_INLINE void setX(b3Scalar _x) { m_floats[0] = _x;}; + /**@brief Set the y value */ + B3_FORCE_INLINE void setY(b3Scalar _y) { m_floats[1] = _y;}; + /**@brief Set the z value */ + B3_FORCE_INLINE void setZ(b3Scalar _z) { m_floats[2] = _z;}; + /**@brief Set the w value */ + B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w;}; + /**@brief Return the x value */ + + + //B3_FORCE_INLINE b3Scalar& operator[](int i) { return (&m_floats[0])[i]; } + //B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; } + ///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. + B3_FORCE_INLINE operator b3Scalar *() { return &m_floats[0]; } + B3_FORCE_INLINE operator const b3Scalar *() const { return &m_floats[0]; } + + B3_FORCE_INLINE bool operator==(const b3QuadWord& other) const + { +#ifdef B3_USE_SSE + return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128))); +#else + return ((m_floats[3]==other.m_floats[3]) && + (m_floats[2]==other.m_floats[2]) && + (m_floats[1]==other.m_floats[1]) && + (m_floats[0]==other.m_floats[0])); +#endif + } + + B3_FORCE_INLINE bool operator!=(const b3QuadWord& other) const + { + return !(*this == other); + } + + /**@brief Set x,y,z and zero w + * @param x Value of x + * @param y Value of y + * @param z Value of z + */ + B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z) + { + m_floats[0]=_x; + m_floats[1]=_y; + m_floats[2]=_z; + m_floats[3] = 0.f; + } + +/* void getValue(b3Scalar *m) const + { + m[0] = m_floats[0]; + m[1] = m_floats[1]; + m[2] = m_floats[2]; + } +*/ +/**@brief Set the values + * @param x Value of x + * @param y Value of y + * @param z Value of z + * @param w Value of w + */ + B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w) + { + m_floats[0]=_x; + m_floats[1]=_y; + m_floats[2]=_z; + m_floats[3]=_w; + } + /**@brief No initialization constructor */ + B3_FORCE_INLINE b3QuadWord() + // :m_floats[0](b3Scalar(0.)),m_floats[1](b3Scalar(0.)),m_floats[2](b3Scalar(0.)),m_floats[3](b3Scalar(0.)) + { + } + + /**@brief Three argument constructor (zeros w) + * @param x Value of x + * @param y Value of y + * @param z Value of z + */ + B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z) + { + m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f; + } + +/**@brief Initializing constructor + * @param x Value of x + * @param y Value of y + * @param z Value of z + * @param w Value of w + */ + B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w) + { + m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w; + } + + /**@brief Set each element to the max of the current values and the values of another b3QuadWord + * @param other The other b3QuadWord to compare with + */ + B3_FORCE_INLINE void setMax(const b3QuadWord& other) + { + #ifdef B3_USE_SSE + mVec128 = _mm_max_ps(mVec128, other.mVec128); + #elif defined(B3_USE_NEON) + mVec128 = vmaxq_f32(mVec128, other.mVec128); + #else + b3SetMax(m_floats[0], other.m_floats[0]); + b3SetMax(m_floats[1], other.m_floats[1]); + b3SetMax(m_floats[2], other.m_floats[2]); + b3SetMax(m_floats[3], other.m_floats[3]); + #endif + } + /**@brief Set each element to the min of the current values and the values of another b3QuadWord + * @param other The other b3QuadWord to compare with + */ + B3_FORCE_INLINE void setMin(const b3QuadWord& other) + { + #ifdef B3_USE_SSE + mVec128 = _mm_min_ps(mVec128, other.mVec128); + #elif defined(B3_USE_NEON) + mVec128 = vminq_f32(mVec128, other.mVec128); + #else + b3SetMin(m_floats[0], other.m_floats[0]); + b3SetMin(m_floats[1], other.m_floats[1]); + b3SetMin(m_floats[2], other.m_floats[2]); + b3SetMin(m_floats[3], other.m_floats[3]); + #endif + } + + + +}; + +#endif //B3_SIMD_QUADWORD_H diff --git a/extern/bullet/src/Bullet3Common/b3Quaternion.h b/extern/bullet/src/Bullet3Common/b3Quaternion.h new file mode 100644 index 000000000000..ad2054334879 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3Quaternion.h @@ -0,0 +1,918 @@ +/* +Copyright (c) 2003-2013 Gino van den Bergen / 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. +*/ + + + +#ifndef B3_SIMD__QUATERNION_H_ +#define B3_SIMD__QUATERNION_H_ + + +#include "b3Vector3.h" +#include "b3QuadWord.h" + + + + + +#ifdef B3_USE_SSE + +const __m128 B3_ATTRIBUTE_ALIGNED16(b3vOnes) = {1.0f, 1.0f, 1.0f, 1.0f}; + +#endif + +#if defined(B3_USE_SSE) || defined(B3_USE_NEON) + +const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f}; +const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f}; + +#endif + +/**@brief The b3Quaternion implements quaternion to perform linear algebra rotations in combination with b3Matrix3x3, b3Vector3 and b3Transform. */ +class b3Quaternion : public b3QuadWord { +public: + /**@brief No initialization constructor */ + b3Quaternion() {} + +#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))|| defined(B3_USE_NEON) + // Set Vector + B3_FORCE_INLINE b3Quaternion(const b3SimdFloat4 vec) + { + mVec128 = vec; + } + + // Copy constructor + B3_FORCE_INLINE b3Quaternion(const b3Quaternion& rhs) + { + mVec128 = rhs.mVec128; + } + + // Assignment Operator + B3_FORCE_INLINE b3Quaternion& + operator=(const b3Quaternion& v) + { + mVec128 = v.mVec128; + + return *this; + } + +#endif + + // template + // explicit Quaternion(const b3Scalar *v) : Tuple4(v) {} + /**@brief Constructor from scalars */ + b3Quaternion(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w) + : b3QuadWord(_x, _y, _z, _w) + { + //b3Assert(!((_x==1.f) && (_y==0.f) && (_z==0.f) && (_w==0.f))); + } + /**@brief Axis angle Constructor + * @param axis The axis which the rotation is around + * @param angle The magnitude of the rotation around the angle (Radians) */ + b3Quaternion(const b3Vector3& _axis, const b3Scalar& _angle) + { + setRotation(_axis, _angle); + } + /**@brief Constructor from Euler angles + * @param yaw Angle around Y unless B3_EULER_DEFAULT_ZYX defined then Z + * @param pitch Angle around X unless B3_EULER_DEFAULT_ZYX defined then Y + * @param roll Angle around Z unless B3_EULER_DEFAULT_ZYX defined then X */ + b3Quaternion(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll) + { +#ifndef B3_EULER_DEFAULT_ZYX + setEuler(yaw, pitch, roll); +#else + setEulerZYX(yaw, pitch, roll); +#endif + } + /**@brief Set the rotation using axis angle notation + * @param axis The axis around which to rotate + * @param angle The magnitude of the rotation in Radians */ + void setRotation(const b3Vector3& axis, const b3Scalar& _angle) + { + b3Scalar d = axis.length(); + b3Assert(d != b3Scalar(0.0)); + b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d; + setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s, + b3Cos(_angle * b3Scalar(0.5))); + } + /**@brief Set the quaternion using Euler angles + * @param yaw Angle around Y + * @param pitch Angle around X + * @param roll Angle around Z */ + void setEuler(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll) + { + b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5); + b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5); + b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5); + b3Scalar cosYaw = b3Cos(halfYaw); + b3Scalar sinYaw = b3Sin(halfYaw); + b3Scalar cosPitch = b3Cos(halfPitch); + b3Scalar sinPitch = b3Sin(halfPitch); + b3Scalar cosRoll = b3Cos(halfRoll); + b3Scalar sinRoll = b3Sin(halfRoll); + setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, + cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, + sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, + cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); + } + + /**@brief Set the quaternion using euler angles + * @param yaw Angle around Z + * @param pitch Angle around Y + * @param roll Angle around X */ + void setEulerZYX(const b3Scalar& yawZ, const b3Scalar& pitchY, const b3Scalar& rollX) + { + b3Scalar halfYaw = b3Scalar(yawZ) * b3Scalar(0.5); + b3Scalar halfPitch = b3Scalar(pitchY) * b3Scalar(0.5); + b3Scalar halfRoll = b3Scalar(rollX) * b3Scalar(0.5); + b3Scalar cosYaw = b3Cos(halfYaw); + b3Scalar sinYaw = b3Sin(halfYaw); + b3Scalar cosPitch = b3Cos(halfPitch); + b3Scalar sinPitch = b3Sin(halfPitch); + b3Scalar cosRoll = b3Cos(halfRoll); + b3Scalar sinRoll = b3Sin(halfRoll); + setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x + cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y + cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z + cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx + normalize(); + } + + /**@brief Get the euler angles from this quaternion + * @param yaw Angle around Z + * @param pitch Angle around Y + * @param roll Angle around X */ + void getEulerZYX(b3Scalar& yawZ, b3Scalar& pitchY, b3Scalar& rollX) const + { + b3Scalar squ; + b3Scalar sqx; + b3Scalar sqy; + b3Scalar sqz; + b3Scalar sarg; + sqx = m_floats[0] * m_floats[0]; + sqy = m_floats[1] * m_floats[1]; + sqz = m_floats[2] * m_floats[2]; + squ = m_floats[3] * m_floats[3]; + rollX = b3Atan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz); + sarg = b3Scalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]); + pitchY = sarg <= b3Scalar(-1.0) ? b3Scalar(-0.5) * B3_PI: (sarg >= b3Scalar(1.0) ? b3Scalar(0.5) * B3_PI : b3Asin(sarg)); + yawZ = b3Atan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz); + } + + /**@brief Add two quaternions + * @param q The quaternion to add to this one */ + B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q) + { +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + mVec128 = _mm_add_ps(mVec128, q.mVec128); +#elif defined(B3_USE_NEON) + mVec128 = vaddq_f32(mVec128, q.mVec128); +#else + m_floats[0] += q.getX(); + m_floats[1] += q.getY(); + m_floats[2] += q.getZ(); + m_floats[3] += q.m_floats[3]; +#endif + return *this; + } + + /**@brief Subtract out a quaternion + * @param q The quaternion to subtract from this one */ + b3Quaternion& operator-=(const b3Quaternion& q) + { +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + mVec128 = _mm_sub_ps(mVec128, q.mVec128); +#elif defined(B3_USE_NEON) + mVec128 = vsubq_f32(mVec128, q.mVec128); +#else + m_floats[0] -= q.getX(); + m_floats[1] -= q.getY(); + m_floats[2] -= q.getZ(); + m_floats[3] -= q.m_floats[3]; +#endif + return *this; + } + + /**@brief Scale this quaternion + * @param s The scalar to scale by */ + b3Quaternion& operator*=(const b3Scalar& s) + { +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 vs = _mm_load_ss(&s); // (S 0 0 0) + vs = b3_pshufd_ps(vs, 0); // (S S S S) + mVec128 = _mm_mul_ps(mVec128, vs); +#elif defined(B3_USE_NEON) + mVec128 = vmulq_n_f32(mVec128, s); +#else + m_floats[0] *= s; + m_floats[1] *= s; + m_floats[2] *= s; + m_floats[3] *= s; +#endif + return *this; + } + + /**@brief Multiply this quaternion by q on the right + * @param q The other quaternion + * Equivilant to this = this * q */ + b3Quaternion& operator*=(const b3Quaternion& q) + { +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 vQ2 = q.get128(); + + __m128 A1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(0,1,2,0)); + __m128 B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3,3,3,0)); + + A1 = A1 * B1; + + __m128 A2 = b3_pshufd_ps(mVec128, B3_SHUFFLE(1,2,0,1)); + __m128 B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2,0,1,1)); + + A2 = A2 * B2; + + B1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(2,0,1,2)); + B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1,2,0,2)); + + B1 = B1 * B2; // A3 *= B3 + + mVec128 = b3_splat_ps(mVec128, 3); // A0 + mVec128 = mVec128 * vQ2; // A0 * B0 + + A1 = A1 + A2; // AB12 + mVec128 = mVec128 - B1; // AB03 = AB0 - AB3 + A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element + mVec128 = mVec128+ A1; // AB03 + AB12 + +#elif defined(B3_USE_NEON) + + float32x4_t vQ1 = mVec128; + float32x4_t vQ2 = q.get128(); + float32x4_t A0, A1, B1, A2, B2, A3, B3; + float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz; + + { + float32x2x2_t tmp; + tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y} + vQ1zx = tmp.val[0]; + + tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y} + vQ2zx = tmp.val[0]; + } + vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1); + + vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1); + + vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1); + vQ2xz = vext_f32(vQ2zx, vQ2zx, 1); + + A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x + B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X + + A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1)); + B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1)); + + A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z + B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z + + A1 = vmulq_f32(A1, B1); + A2 = vmulq_f32(A2, B2); + A3 = vmulq_f32(A3, B3); // A3 *= B3 + A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0 + + A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2 + A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3 + + // change the sign of the last element + A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM); + A0 = vaddq_f32(A0, A1); // AB03 + AB12 + + mVec128 = A0; +#else + setValue( + m_floats[3] * q.getX() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.getZ() - m_floats[2] * q.getY(), + m_floats[3] * q.getY() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.getX() - m_floats[0] * q.getZ(), + m_floats[3] * q.getZ() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.getY() - m_floats[1] * q.getX(), + m_floats[3] * q.m_floats[3] - m_floats[0] * q.getX() - m_floats[1] * q.getY() - m_floats[2] * q.getZ()); +#endif + return *this; + } + /**@brief Return the dot product between this quaternion and another + * @param q The other quaternion */ + b3Scalar dot(const b3Quaternion& q) const + { +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 vd; + + vd = _mm_mul_ps(mVec128, q.mVec128); + + __m128 t = _mm_movehl_ps(vd, vd); + vd = _mm_add_ps(vd, t); + t = _mm_shuffle_ps(vd, vd, 0x55); + vd = _mm_add_ss(vd, t); + + return _mm_cvtss_f32(vd); +#elif defined(B3_USE_NEON) + float32x4_t vd = vmulq_f32(mVec128, q.mVec128); + float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd)); + x = vpadd_f32(x, x); + return vget_lane_f32(x, 0); +#else + return m_floats[0] * q.getX() + + m_floats[1] * q.getY() + + m_floats[2] * q.getZ() + + m_floats[3] * q.m_floats[3]; +#endif + } + + /**@brief Return the length squared of the quaternion */ + b3Scalar length2() const + { + return dot(*this); + } + + /**@brief Return the length of the quaternion */ + b3Scalar length() const + { + return b3Sqrt(length2()); + } + + /**@brief Normalize the quaternion + * Such that x^2 + y^2 + z^2 +w^2 = 1 */ + b3Quaternion& normalize() + { +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 vd; + + vd = _mm_mul_ps(mVec128, mVec128); + + __m128 t = _mm_movehl_ps(vd, vd); + vd = _mm_add_ps(vd, t); + t = _mm_shuffle_ps(vd, vd, 0x55); + vd = _mm_add_ss(vd, t); + + vd = _mm_sqrt_ss(vd); + vd = _mm_div_ss(b3vOnes, vd); + vd = b3_pshufd_ps(vd, 0); // splat + mVec128 = _mm_mul_ps(mVec128, vd); + + return *this; +#else + return *this /= length(); +#endif + } + + /**@brief Return a scaled version of this quaternion + * @param s The scale factor */ + B3_FORCE_INLINE b3Quaternion + operator*(const b3Scalar& s) const + { +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 vs = _mm_load_ss(&s); // (S 0 0 0) + vs = b3_pshufd_ps(vs, 0x00); // (S S S S) + + return b3Quaternion(_mm_mul_ps(mVec128, vs)); +#elif defined(B3_USE_NEON) + return b3Quaternion(vmulq_n_f32(mVec128, s)); +#else + return b3Quaternion(getX() * s, getY() * s, getZ() * s, m_floats[3] * s); +#endif + } + + /**@brief Return an inversely scaled versionof this quaternion + * @param s The inverse scale factor */ + b3Quaternion operator/(const b3Scalar& s) const + { + b3Assert(s != b3Scalar(0.0)); + return *this * (b3Scalar(1.0) / s); + } + + /**@brief Inversely scale this quaternion + * @param s The scale factor */ + b3Quaternion& operator/=(const b3Scalar& s) + { + b3Assert(s != b3Scalar(0.0)); + return *this *= b3Scalar(1.0) / s; + } + + /**@brief Return a normalized version of this quaternion */ + b3Quaternion normalized() const + { + return *this / length(); + } + /**@brief Return the angle between this quaternion and the other + * @param q The other quaternion */ + b3Scalar angle(const b3Quaternion& q) const + { + b3Scalar s = b3Sqrt(length2() * q.length2()); + b3Assert(s != b3Scalar(0.0)); + return b3Acos(dot(q) / s); + } + /**@brief Return the angle of rotation represented by this quaternion */ + b3Scalar getAngle() const + { + b3Scalar s = b3Scalar(2.) * b3Acos(m_floats[3]); + return s; + } + + /**@brief Return the axis of the rotation represented by this quaternion */ + b3Vector3 getAxis() const + { + b3Scalar s_squared = 1.f-m_floats[3]*m_floats[3]; + + if (s_squared < b3Scalar(10.) * B3_EPSILON) //Check for divide by zero + return b3MakeVector3(1.0, 0.0, 0.0); // Arbitrary + b3Scalar s = 1.f/b3Sqrt(s_squared); + return b3MakeVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s); + } + + /**@brief Return the inverse of this quaternion */ + b3Quaternion inverse() const + { +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + return b3Quaternion(_mm_xor_ps(mVec128, b3vQInv)); +#elif defined(B3_USE_NEON) + return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)b3vQInv)); +#else + return b3Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); +#endif + } + + /**@brief Return the sum of this quaternion and the other + * @param q2 The other quaternion */ + B3_FORCE_INLINE b3Quaternion + operator+(const b3Quaternion& q2) const + { +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + return b3Quaternion(_mm_add_ps(mVec128, q2.mVec128)); +#elif defined(B3_USE_NEON) + return b3Quaternion(vaddq_f32(mVec128, q2.mVec128)); +#else + const b3Quaternion& q1 = *this; + return b3Quaternion(q1.getX() + q2.getX(), q1.getY() + q2.getY(), q1.getZ() + q2.getZ(), q1.m_floats[3] + q2.m_floats[3]); +#endif + } + + /**@brief Return the difference between this quaternion and the other + * @param q2 The other quaternion */ + B3_FORCE_INLINE b3Quaternion + operator-(const b3Quaternion& q2) const + { +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + return b3Quaternion(_mm_sub_ps(mVec128, q2.mVec128)); +#elif defined(B3_USE_NEON) + return b3Quaternion(vsubq_f32(mVec128, q2.mVec128)); +#else + const b3Quaternion& q1 = *this; + return b3Quaternion(q1.getX() - q2.getX(), q1.getY() - q2.getY(), q1.getZ() - q2.getZ(), q1.m_floats[3] - q2.m_floats[3]); +#endif + } + + /**@brief Return the negative of this quaternion + * This simply negates each element */ + B3_FORCE_INLINE b3Quaternion operator-() const + { +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + return b3Quaternion(_mm_xor_ps(mVec128, b3vMzeroMask)); +#elif defined(B3_USE_NEON) + return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)b3vMzeroMask) ); +#else + const b3Quaternion& q2 = *this; + return b3Quaternion( - q2.getX(), - q2.getY(), - q2.getZ(), - q2.m_floats[3]); +#endif + } + /**@todo document this and it's use */ + B3_FORCE_INLINE b3Quaternion farthest( const b3Quaternion& qd) const + { + b3Quaternion diff,sum; + diff = *this - qd; + sum = *this + qd; + if( diff.dot(diff) > sum.dot(sum) ) + return qd; + return (-qd); + } + + /**@todo document this and it's use */ + B3_FORCE_INLINE b3Quaternion nearest( const b3Quaternion& qd) const + { + b3Quaternion diff,sum; + diff = *this - qd; + sum = *this + qd; + if( diff.dot(diff) < sum.dot(sum) ) + return qd; + return (-qd); + } + + + /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion + * @param q The other quaternion to interpolate with + * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. + * Slerp interpolates assuming constant velocity. */ + b3Quaternion slerp(const b3Quaternion& q, const b3Scalar& t) const + { + b3Scalar magnitude = b3Sqrt(length2() * q.length2()); + b3Assert(magnitude > b3Scalar(0)); + + b3Scalar product = dot(q) / magnitude; + if (b3Fabs(product) < b3Scalar(1)) + { + // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp + const b3Scalar sign = (product < 0) ? b3Scalar(-1) : b3Scalar(1); + + const b3Scalar theta = b3Acos(sign * product); + const b3Scalar s1 = b3Sin(sign * t * theta); + const b3Scalar d = b3Scalar(1.0) / b3Sin(theta); + const b3Scalar s0 = b3Sin((b3Scalar(1.0) - t) * theta); + + return b3Quaternion( + (m_floats[0] * s0 + q.getX() * s1) * d, + (m_floats[1] * s0 + q.getY() * s1) * d, + (m_floats[2] * s0 + q.getZ() * s1) * d, + (m_floats[3] * s0 + q.m_floats[3] * s1) * d); + } + else + { + return *this; + } + } + + static const b3Quaternion& getIdentity() + { + static const b3Quaternion identityQuat(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.),b3Scalar(1.)); + return identityQuat; + } + + B3_FORCE_INLINE const b3Scalar& getW() const { return m_floats[3]; } + + +}; + + + + + +/**@brief Return the product of two quaternions */ +B3_FORCE_INLINE b3Quaternion +operator*(const b3Quaternion& q1, const b3Quaternion& q2) +{ +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 vQ1 = q1.get128(); + __m128 vQ2 = q2.get128(); + __m128 A0, A1, B1, A2, B2; + + A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0,1,2,0)); // X Y z x // vtrn + B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3,3,3,0)); // W W W X // vdup vext + + A1 = A1 * B1; + + A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1,2,0,1)); // Y Z X Y // vext + B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2,0,1,1)); // z x Y Y // vtrn vdup + + A2 = A2 * B2; + + B1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2,0,1,2)); // z x Y Z // vtrn vext + B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1,2,0,2)); // Y Z x z // vext vtrn + + B1 = B1 * B2; // A3 *= B3 + + A0 = b3_splat_ps(vQ1, 3); // A0 + A0 = A0 * vQ2; // A0 * B0 + + A1 = A1 + A2; // AB12 + A0 = A0 - B1; // AB03 = AB0 - AB3 + + A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element + A0 = A0 + A1; // AB03 + AB12 + + return b3Quaternion(A0); + +#elif defined(B3_USE_NEON) + + float32x4_t vQ1 = q1.get128(); + float32x4_t vQ2 = q2.get128(); + float32x4_t A0, A1, B1, A2, B2, A3, B3; + float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz; + + { + float32x2x2_t tmp; + tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y} + vQ1zx = tmp.val[0]; + + tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y} + vQ2zx = tmp.val[0]; + } + vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1); + + vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1); + + vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1); + vQ2xz = vext_f32(vQ2zx, vQ2zx, 1); + + A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x + B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X + + A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1)); + B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1)); + + A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z + B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z + + A1 = vmulq_f32(A1, B1); + A2 = vmulq_f32(A2, B2); + A3 = vmulq_f32(A3, B3); // A3 *= B3 + A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0 + + A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2 + A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3 + + // change the sign of the last element + A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM); + A0 = vaddq_f32(A0, A1); // AB03 + AB12 + + return b3Quaternion(A0); + +#else + return b3Quaternion( + q1.getW() * q2.getX() + q1.getX() * q2.getW() + q1.getY() * q2.getZ() - q1.getZ() * q2.getY(), + q1.getW() * q2.getY() + q1.getY() * q2.getW() + q1.getZ() * q2.getX() - q1.getX() * q2.getZ(), + q1.getW() * q2.getZ() + q1.getZ() * q2.getW() + q1.getX() * q2.getY() - q1.getY() * q2.getX(), + q1.getW() * q2.getW() - q1.getX() * q2.getX() - q1.getY() * q2.getY() - q1.getZ() * q2.getZ()); +#endif +} + +B3_FORCE_INLINE b3Quaternion +operator*(const b3Quaternion& q, const b3Vector3& w) +{ +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 vQ1 = q.get128(); + __m128 vQ2 = w.get128(); + __m128 A1, B1, A2, B2, A3, B3; + + A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(3,3,3,0)); + B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(0,1,2,0)); + + A1 = A1 * B1; + + A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1,2,0,1)); + B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2,0,1,1)); + + A2 = A2 * B2; + + A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2,0,1,2)); + B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1,2,0,2)); + + A3 = A3 * B3; // A3 *= B3 + + A1 = A1 + A2; // AB12 + A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element + A1 = A1 - A3; // AB123 = AB12 - AB3 + + return b3Quaternion(A1); + +#elif defined(B3_USE_NEON) + + float32x4_t vQ1 = q.get128(); + float32x4_t vQ2 = w.get128(); + float32x4_t A1, B1, A2, B2, A3, B3; + float32x2_t vQ1wx, vQ2zx, vQ1yz, vQ2yz, vQ1zx, vQ2xz; + + vQ1wx = vext_f32(vget_high_f32(vQ1), vget_low_f32(vQ1), 1); + { + float32x2x2_t tmp; + + tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y} + vQ2zx = tmp.val[0]; + + tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y} + vQ1zx = tmp.val[0]; + } + + vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1); + + vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1); + vQ2xz = vext_f32(vQ2zx, vQ2zx, 1); + + A1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ1), 1), vQ1wx); // W W W X + B1 = vcombine_f32(vget_low_f32(vQ2), vQ2zx); // X Y z x + + A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1)); + B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1)); + + A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z + B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z + + A1 = vmulq_f32(A1, B1); + A2 = vmulq_f32(A2, B2); + A3 = vmulq_f32(A3, B3); // A3 *= B3 + + A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2 + + // change the sign of the last element + A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM); + + A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3 + + return b3Quaternion(A1); + +#else + return b3Quaternion( + q.getW() * w.getX() + q.getY() * w.getZ() - q.getZ() * w.getY(), + q.getW() * w.getY() + q.getZ() * w.getX() - q.getX() * w.getZ(), + q.getW() * w.getZ() + q.getX() * w.getY() - q.getY() * w.getX(), + -q.getX() * w.getX() - q.getY() * w.getY() - q.getZ() * w.getZ()); +#endif +} + +B3_FORCE_INLINE b3Quaternion +operator*(const b3Vector3& w, const b3Quaternion& q) +{ +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 vQ1 = w.get128(); + __m128 vQ2 = q.get128(); + __m128 A1, B1, A2, B2, A3, B3; + + A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0,1,2,0)); // X Y z x + B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3,3,3,0)); // W W W X + + A1 = A1 * B1; + + A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1,2,0,1)); + B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2,0,1,1)); + + A2 = A2 *B2; + + A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2,0,1,2)); + B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1,2,0,2)); + + A3 = A3 * B3; // A3 *= B3 + + A1 = A1 + A2; // AB12 + A1 = _mm_xor_ps(A1, b3vPPPM); // change sign of the last element + A1 = A1 - A3; // AB123 = AB12 - AB3 + + return b3Quaternion(A1); + +#elif defined(B3_USE_NEON) + + float32x4_t vQ1 = w.get128(); + float32x4_t vQ2 = q.get128(); + float32x4_t A1, B1, A2, B2, A3, B3; + float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz; + + { + float32x2x2_t tmp; + + tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y} + vQ1zx = tmp.val[0]; + + tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y} + vQ2zx = tmp.val[0]; + } + vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1); + + vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1); + + vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1); + vQ2xz = vext_f32(vQ2zx, vQ2zx, 1); + + A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x + B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X + + A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1)); + B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1)); + + A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z + B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z + + A1 = vmulq_f32(A1, B1); + A2 = vmulq_f32(A2, B2); + A3 = vmulq_f32(A3, B3); // A3 *= B3 + + A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2 + + // change the sign of the last element + A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM); + + A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3 + + return b3Quaternion(A1); + +#else + return b3Quaternion( + +w.getX() * q.getW() + w.getY() * q.getZ() - w.getZ() * q.getY(), + +w.getY() * q.getW() + w.getZ() * q.getX() - w.getX() * q.getZ(), + +w.getZ() * q.getW() + w.getX() * q.getY() - w.getY() * q.getX(), + -w.getX() * q.getX() - w.getY() * q.getY() - w.getZ() * q.getZ()); +#endif +} + +/**@brief Calculate the dot product between two quaternions */ +B3_FORCE_INLINE b3Scalar +b3Dot(const b3Quaternion& q1, const b3Quaternion& q2) +{ + return q1.dot(q2); +} + + +/**@brief Return the length of a quaternion */ +B3_FORCE_INLINE b3Scalar +b3Length(const b3Quaternion& q) +{ + return q.length(); +} + +/**@brief Return the angle between two quaternions*/ +B3_FORCE_INLINE b3Scalar +b3Angle(const b3Quaternion& q1, const b3Quaternion& q2) +{ + return q1.angle(q2); +} + +/**@brief Return the inverse of a quaternion*/ +B3_FORCE_INLINE b3Quaternion +b3Inverse(const b3Quaternion& q) +{ + return q.inverse(); +} + +/**@brief Return the result of spherical linear interpolation betwen two quaternions + * @param q1 The first quaternion + * @param q2 The second quaternion + * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2 + * Slerp assumes constant velocity between positions. */ +B3_FORCE_INLINE b3Quaternion +b3Slerp(const b3Quaternion& q1, const b3Quaternion& q2, const b3Scalar& t) +{ + return q1.slerp(q2, t); +} + +B3_FORCE_INLINE b3Quaternion +b3QuatMul(const b3Quaternion& rot0, const b3Quaternion& rot1) +{ + return rot0*rot1; +} + +B3_FORCE_INLINE b3Quaternion +b3QuatNormalized(const b3Quaternion& orn) +{ + return orn.normalized(); +} + + + +B3_FORCE_INLINE b3Vector3 +b3QuatRotate(const b3Quaternion& rotation, const b3Vector3& v) +{ + b3Quaternion q = rotation * v; + q *= rotation.inverse(); +#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + return b3MakeVector3(_mm_and_ps(q.get128(), b3vFFF0fMask)); +#elif defined(B3_USE_NEON) + return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), b3vFFF0Mask)); +#else + return b3MakeVector3(q.getX(),q.getY(),q.getZ()); +#endif +} + +B3_FORCE_INLINE b3Quaternion +b3ShortestArcQuat(const b3Vector3& v0, const b3Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized +{ + b3Vector3 c = v0.cross(v1); + b3Scalar d = v0.dot(v1); + + if (d < -1.0 + B3_EPSILON) + { + b3Vector3 n,unused; + b3PlaneSpace1(v0,n,unused); + return b3Quaternion(n.getX(),n.getY(),n.getZ(),0.0f); // just pick any vector that is orthogonal to v0 + } + + b3Scalar s = b3Sqrt((1.0f + d) * 2.0f); + b3Scalar rs = 1.0f / s; + + return b3Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f); + +} + +B3_FORCE_INLINE b3Quaternion +b3ShortestArcQuatNormalize2(b3Vector3& v0,b3Vector3& v1) +{ + v0.normalize(); + v1.normalize(); + return b3ShortestArcQuat(v0,v1); +} + +#endif //B3_SIMD__QUATERNION_H_ + + + diff --git a/extern/bullet/src/Bullet3Common/b3Random.h b/extern/bullet/src/Bullet3Common/b3Random.h new file mode 100644 index 000000000000..dc040f15626d --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3Random.h @@ -0,0 +1,50 @@ +/* +Copyright (c) 2003-2013 Gino van den Bergen / 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. +*/ + + + +#ifndef B3_GEN_RANDOM_H +#define B3_GEN_RANDOM_H + +#include "b3Scalar.h" + +#ifdef MT19937 + +#include +#include + +#define B3_RAND_MAX UINT_MAX + +B3_FORCE_INLINE void b3Srand(unsigned int seed) { init_genrand(seed); } +B3_FORCE_INLINE unsigned int b3rand() { return genrand_int32(); } + +#else + +#include + +#define B3_RAND_MAX RAND_MAX + +B3_FORCE_INLINE void b3Srand(unsigned int seed) { srand(seed); } +B3_FORCE_INLINE unsigned int b3rand() { return rand(); } + +#endif + +inline b3Scalar b3RandRange(b3Scalar minRange, b3Scalar maxRange) +{ + return (b3rand() / (b3Scalar(B3_RAND_MAX) + b3Scalar(1.0))) * (maxRange - minRange) + minRange; +} + + +#endif //B3_GEN_RANDOM_H + diff --git a/extern/bullet/src/Bullet3Common/b3ResizablePool.h b/extern/bullet/src/Bullet3Common/b3ResizablePool.h new file mode 100644 index 000000000000..06ad8a778d38 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3ResizablePool.h @@ -0,0 +1,182 @@ + +#ifndef B3_RESIZABLE_POOL_H +#define B3_RESIZABLE_POOL_H + +#include "Bullet3Common/b3AlignedObjectArray.h" + +enum +{ + B3_POOL_HANDLE_TERMINAL_FREE=-1, + B3_POOL_HANDLE_TERMINAL_USED =-2 +}; + +template +struct b3PoolBodyHandle : public U +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); + + int m_nextFreeHandle; + void setNextFree(int next) + { + m_nextFreeHandle = next; + } + int getNextFree() const + { + return m_nextFreeHandle; + } +}; + +template +class b3ResizablePool +{ + +protected: + b3AlignedObjectArray m_bodyHandles; + int m_numUsedHandles; // number of active handles + int m_firstFreeHandle; // free handles list + + T* getHandleInternal(int handle) + { + return &m_bodyHandles[handle]; + + } + const T* getHandleInternal(int handle) const + { + return &m_bodyHandles[handle]; + } + +public: + + b3ResizablePool() + { + initHandles(); + } + + virtual ~b3ResizablePool() + { + exitHandles(); + } +///handle management + + int getNumHandles() const + { + return m_bodyHandles.size(); + } + + void getUsedHandles(b3AlignedObjectArray& usedHandles) const + { + + for (int i=0;i=0); + b3Assert(handle=m_bodyHandles.size())) + { + return 0; + } + + if (m_bodyHandles[handle].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED) + { + return &m_bodyHandles[handle]; + } + return 0; + + } + const T* getHandle(int handle) const + { + b3Assert(handle>=0); + b3Assert(handle=m_bodyHandles.size())) + { + return 0; + } + + if (m_bodyHandles[handle].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED) + { + return &m_bodyHandles[handle]; + } + return 0; + } + + void increaseHandleCapacity(int extraCapacity) + { + int curCapacity = m_bodyHandles.size(); + //b3Assert(curCapacity == m_numUsedHandles); + int newCapacity = curCapacity + extraCapacity; + m_bodyHandles.resize(newCapacity); + + { + for (int i = curCapacity; i < newCapacity; i++) + m_bodyHandles[i].setNextFree(i + 1); + + + m_bodyHandles[newCapacity - 1].setNextFree(-1); + } + m_firstFreeHandle = curCapacity; + } + void initHandles() + { + m_numUsedHandles = 0; + m_firstFreeHandle = -1; + + increaseHandleCapacity(1); + } + + void exitHandles() + { + m_bodyHandles.resize(0); + m_firstFreeHandle = -1; + m_numUsedHandles = 0; + } + + int allocHandle() + { + b3Assert(m_firstFreeHandle>=0); + + int handle = m_firstFreeHandle; + m_firstFreeHandle = getHandleInternal(handle)->getNextFree(); + m_numUsedHandles++; + + if (m_firstFreeHandle<0) + { + //int curCapacity = m_bodyHandles.size(); + int additionalCapacity= m_bodyHandles.size(); + increaseHandleCapacity(additionalCapacity); + + + getHandleInternal(handle)->setNextFree(m_firstFreeHandle); + } + getHandleInternal(handle)->setNextFree(B3_POOL_HANDLE_TERMINAL_USED); + getHandleInternal(handle)->clear(); + return handle; + } + + + void freeHandle(int handle) + { + b3Assert(handle >= 0); + + if (m_bodyHandles[handle].getNextFree()==B3_POOL_HANDLE_TERMINAL_USED) + { + getHandleInternal(handle)->clear(); + getHandleInternal(handle)->setNextFree(m_firstFreeHandle); + m_firstFreeHandle = handle; + m_numUsedHandles--; + } + } +}; + ///end handle management + + #endif //B3_RESIZABLE_POOL_H + \ No newline at end of file diff --git a/extern/bullet/src/Bullet3Common/b3Scalar.h b/extern/bullet/src/Bullet3Common/b3Scalar.h new file mode 100644 index 000000000000..dbc7fea397d8 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3Scalar.h @@ -0,0 +1,663 @@ +/* +Copyright (c) 2003-2013 Gino van den Bergen / 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. +*/ + + + +#ifndef B3_SCALAR_H +#define B3_SCALAR_H + +#ifdef B3_MANAGED_CODE +//Aligned data types not supported in managed code +#pragma unmanaged +#endif + + + +#include +#include //size_t for MSVC 6.0 +#include + +//Original repository is at http://github.com/erwincoumans/bullet3 +#define B3_BULLET_VERSION 300 + +inline int b3GetVersion() +{ + return B3_BULLET_VERSION; +} + +#if defined(DEBUG) || defined (_DEBUG) +#define B3_DEBUG +#endif + +#include "b3Logging.h"//for b3Error + + +#ifdef _WIN32 + + #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) + + #define B3_FORCE_INLINE inline + #define B3_ATTRIBUTE_ALIGNED16(a) a + #define B3_ATTRIBUTE_ALIGNED64(a) a + #define B3_ATTRIBUTE_ALIGNED128(a) a + #else + //#define B3_HAS_ALIGNED_ALLOCATOR + #pragma warning(disable : 4324) // disable padding warning +// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. + #pragma warning(disable:4996) //Turn off warnings about deprecated C routines +// #pragma warning(disable:4786) // Disable the "debug name too long" warning + + #define B3_FORCE_INLINE __forceinline + #define B3_ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a + #define B3_ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a + #define B3_ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a + #ifdef _XBOX + #define B3_USE_VMX128 + + #include + #define B3_HAVE_NATIVE_FSEL + #define b3Fsel(a,b,c) __fsel((a),(b),(c)) + #else + +#if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (B3_USE_DOUBLE_PRECISION)) + #if (defined (_M_IX86) || defined (_M_X64)) + #define B3_USE_SSE + #ifdef B3_USE_SSE + //B3_USE_SSE_IN_API is disabled under Windows by default, because + //it makes it harder to integrate Bullet into your application under Windows + //(structured embedding Bullet structs/classes need to be 16-byte aligned) + //with relatively little performance gain + //If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries + //you can manually enable this line or set it in the build system for a bit of performance gain (a few percent, dependent on usage) + //#define B3_USE_SSE_IN_API + #endif //B3_USE_SSE + #include + #endif +#endif + + #endif//_XBOX + + #endif //__MINGW32__ + +#ifdef B3_DEBUG + #ifdef _MSC_VER + #include + #define b3Assert(x) { if(!(x)){b3Error("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak(); }} + #else//_MSC_VER + #include + #define b3Assert assert + #endif//_MSC_VER +#else + #define b3Assert(x) +#endif + //b3FullAssert is optional, slows down a lot + #define b3FullAssert(x) + + #define b3Likely(_c) _c + #define b3Unlikely(_c) _c + +#else + +#if defined (__CELLOS_LV2__) + #define B3_FORCE_INLINE inline __attribute__((always_inline)) + #define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + #define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #ifndef assert + #include + #endif +#ifdef B3_DEBUG +#ifdef __SPU__ +#include +#define printf spu_printf + #define b3Assert(x) {if(!(x)){b3Error("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}} +#else + #define b3Assert assert +#endif + +#else + #define b3Assert(x) +#endif + //b3FullAssert is optional, slows down a lot + #define b3FullAssert(x) + + #define b3Likely(_c) _c + #define b3Unlikely(_c) _c + +#else + +#ifdef USE_LIBSPE2 + + #define B3_FORCE_INLINE __inline + #define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + #define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #ifndef assert + #include + #endif +#ifdef B3_DEBUG + #define b3Assert assert +#else + #define b3Assert(x) +#endif + //b3FullAssert is optional, slows down a lot + #define b3FullAssert(x) + + + #define b3Likely(_c) __builtin_expect((_c), 1) + #define b3Unlikely(_c) __builtin_expect((_c), 0) + + +#else + //non-windows systems + +#if (defined (__APPLE__) && (!defined (B3_USE_DOUBLE_PRECISION))) + #if defined (__i386__) || defined (__x86_64__) + #define B3_USE_SSE + //B3_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries + //if apps run into issues, we will disable the next line + #define B3_USE_SSE_IN_API + #ifdef B3_USE_SSE + // include appropriate SSE level + #if defined (__SSE4_1__) + #include + #elif defined (__SSSE3__) + #include + #elif defined (__SSE3__) + #include + #else + #include + #endif + #endif //B3_USE_SSE + #elif defined( __armv7__ ) + #ifdef __clang__ + #define B3_USE_NEON 1 + + #if defined B3_USE_NEON && defined (__clang__) + #include + #endif//B3_USE_NEON + #endif //__clang__ + #endif//__arm__ + + #define B3_FORCE_INLINE inline __attribute__ ((always_inline)) +///@todo: check out alignment methods for other platforms/compilers + #define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + #define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #ifndef assert + #include + #endif + + #if defined(DEBUG) || defined (_DEBUG) + #if defined (__i386__) || defined (__x86_64__) + #include + #define b3Assert(x)\ + {\ + if(!(x))\ + {\ + b3Error("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\ + asm volatile ("int3");\ + }\ + } + #else//defined (__i386__) || defined (__x86_64__) + #define b3Assert assert + #endif//defined (__i386__) || defined (__x86_64__) + #else//defined(DEBUG) || defined (_DEBUG) + #define b3Assert(x) + #endif//defined(DEBUG) || defined (_DEBUG) + + //b3FullAssert is optional, slows down a lot + #define b3FullAssert(x) + #define b3Likely(_c) _c + #define b3Unlikely(_c) _c + +#else + + #define B3_FORCE_INLINE inline + ///@todo: check out alignment methods for other platforms/compilers + #define B3_ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define B3_ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + #define B3_ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + ///#define B3_ATTRIBUTE_ALIGNED16(a) a + ///#define B3_ATTRIBUTE_ALIGNED64(a) a + ///#define B3_ATTRIBUTE_ALIGNED128(a) a + #ifndef assert + #include + #endif + +#if defined(DEBUG) || defined (_DEBUG) + #define b3Assert assert +#else + #define b3Assert(x) +#endif + + //b3FullAssert is optional, slows down a lot + #define b3FullAssert(x) + #define b3Likely(_c) _c + #define b3Unlikely(_c) _c +#endif //__APPLE__ + +#endif // LIBSPE2 + +#endif //__CELLOS_LV2__ +#endif + + +///The b3Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision. +#if defined(B3_USE_DOUBLE_PRECISION) +typedef double b3Scalar; +//this number could be bigger in double precision +#define B3_LARGE_FLOAT 1e30 +#else +typedef float b3Scalar; +//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX +#define B3_LARGE_FLOAT 1e18f +#endif + +#ifdef B3_USE_SSE +typedef __m128 b3SimdFloat4; +#endif//B3_USE_SSE + +#if defined B3_USE_SSE_IN_API && defined (B3_USE_SSE) +#ifdef _WIN32 + +#ifndef B3_NAN +static int b3NanMask = 0x7F800001; +#define B3_NAN (*(float*)&b3NanMask) +#endif + +#ifndef B3_INFINITY_MASK +static int b3InfinityMask = 0x7F800000; +#define B3_INFINITY_MASK (*(float*)&b3InfinityMask) +#endif + +inline __m128 operator + (const __m128 A, const __m128 B) +{ + return _mm_add_ps(A, B); +} + +inline __m128 operator - (const __m128 A, const __m128 B) +{ + return _mm_sub_ps(A, B); +} + +inline __m128 operator * (const __m128 A, const __m128 B) +{ + return _mm_mul_ps(A, B); +} + +#define b3CastfTo128i(a) (_mm_castps_si128(a)) +#define b3CastfTo128d(a) (_mm_castps_pd(a)) +#define b3CastiTo128f(a) (_mm_castsi128_ps(a)) +#define b3CastdTo128f(a) (_mm_castpd_ps(a)) +#define b3CastdTo128i(a) (_mm_castpd_si128(a)) +#define b3Assign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3) + +#else//_WIN32 + +#define b3CastfTo128i(a) ((__m128i)(a)) +#define b3CastfTo128d(a) ((__m128d)(a)) +#define b3CastiTo128f(a) ((__m128) (a)) +#define b3CastdTo128f(a) ((__m128) (a)) +#define b3CastdTo128i(a) ((__m128i)(a)) +#define b3Assign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3} +#endif//_WIN32 +#endif //B3_USE_SSE_IN_API + +#ifdef B3_USE_NEON +#include + +typedef float32x4_t b3SimdFloat4; +#define B3_INFINITY INFINITY +#define B3_NAN NAN +#define b3Assign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3} +#endif + + + + + +#define B3_DECLARE_ALIGNED_ALLOCATOR() \ + B3_FORCE_INLINE void* operator new(size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes,16); } \ + B3_FORCE_INLINE void operator delete(void* ptr) { b3AlignedFree(ptr); } \ + B3_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \ + B3_FORCE_INLINE void operator delete(void*, void*) { } \ + B3_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes,16); } \ + B3_FORCE_INLINE void operator delete[](void* ptr) { b3AlignedFree(ptr); } \ + B3_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \ + B3_FORCE_INLINE void operator delete[](void*, void*) { } \ + + + +#if defined(B3_USE_DOUBLE_PRECISION) || defined(B3_FORCE_DOUBLE_FUNCTIONS) + +B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar x) { return sqrt(x); } +B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabs(x); } +B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cos(x); } +B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sin(x); } +B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tan(x); } +B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x) { if (xb3Scalar(1)) x=b3Scalar(1); return acos(x); } +B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x) { if (xb3Scalar(1)) x=b3Scalar(1); return asin(x); } +B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atan(x); } +B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2(x, y); } +B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return exp(x); } +B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return log(x); } +B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x,b3Scalar y) { return pow(x,y); } +B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x,b3Scalar y) { return fmod(x,y); } + +#else + +B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar y) +{ +#ifdef USE_APPROXIMATION + double x, z, tempf; + unsigned long *tfptr = ((unsigned long *)&tempf) + 1; + + tempf = y; + *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ + x = tempf; + z = y*b3Scalar(0.5); + x = (b3Scalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */ + x = (b3Scalar(1.5)*x)-(x*x)*(x*z); + x = (b3Scalar(1.5)*x)-(x*x)*(x*z); + x = (b3Scalar(1.5)*x)-(x*x)*(x*z); + x = (b3Scalar(1.5)*x)-(x*x)*(x*z); + return x*y; +#else + return sqrtf(y); +#endif +} +B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabsf(x); } +B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cosf(x); } +B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sinf(x); } +B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tanf(x); } +B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x) { + if (xb3Scalar(1)) + x=b3Scalar(1); + return acosf(x); +} +B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x) { + if (xb3Scalar(1)) + x=b3Scalar(1); + return asinf(x); +} +B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atanf(x); } +B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2f(x, y); } +B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return expf(x); } +B3_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return logf(x); } +B3_FORCE_INLINE b3Scalar b3Pow(b3Scalar x,b3Scalar y) { return powf(x,y); } +B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x,b3Scalar y) { return fmodf(x,y); } + +#endif + +#define B3_2_PI b3Scalar(6.283185307179586232) +#define B3_PI (B3_2_PI * b3Scalar(0.5)) +#define B3_HALF_PI (B3_2_PI * b3Scalar(0.25)) +#define B3_RADS_PER_DEG (B3_2_PI / b3Scalar(360.0)) +#define B3_DEGS_PER_RAD (b3Scalar(360.0) / B3_2_PI) +#define B3_SQRT12 b3Scalar(0.7071067811865475244008443621048490) + +#define b3RecipSqrt(x) ((b3Scalar)(b3Scalar(1.0)/b3Sqrt(b3Scalar(x)))) /* reciprocal square root */ + + +#ifdef B3_USE_DOUBLE_PRECISION +#define B3_EPSILON DBL_EPSILON +#define B3_INFINITY DBL_MAX +#else +#define B3_EPSILON FLT_EPSILON +#define B3_INFINITY FLT_MAX +#endif + +B3_FORCE_INLINE b3Scalar b3Atan2Fast(b3Scalar y, b3Scalar x) +{ + b3Scalar coeff_1 = B3_PI / 4.0f; + b3Scalar coeff_2 = 3.0f * coeff_1; + b3Scalar abs_y = b3Fabs(y); + b3Scalar angle; + if (x >= 0.0f) { + b3Scalar r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } else { + b3Scalar r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + return (y < 0.0f) ? -angle : angle; +} + +B3_FORCE_INLINE bool b3FuzzyZero(b3Scalar x) { return b3Fabs(x) < B3_EPSILON; } + +B3_FORCE_INLINE bool b3Equal(b3Scalar a, b3Scalar eps) { + return (((a) <= eps) && !((a) < -eps)); +} +B3_FORCE_INLINE bool b3GreaterEqual (b3Scalar a, b3Scalar eps) { + return (!((a) <= eps)); +} + + +B3_FORCE_INLINE int b3IsNegative(b3Scalar x) { + return x < b3Scalar(0.0) ? 1 : 0; +} + +B3_FORCE_INLINE b3Scalar b3Radians(b3Scalar x) { return x * B3_RADS_PER_DEG; } +B3_FORCE_INLINE b3Scalar b3Degrees(b3Scalar x) { return x * B3_DEGS_PER_RAD; } + +#define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name + +#ifndef b3Fsel +B3_FORCE_INLINE b3Scalar b3Fsel(b3Scalar a, b3Scalar b, b3Scalar c) +{ + return a >= 0 ? b : c; +} +#endif +#define b3Fsels(a,b,c) (b3Scalar)b3Fsel(a,b,c) + + +B3_FORCE_INLINE bool b3MachineIsLittleEndian() +{ + long int i = 1; + const char *p = (const char *) &i; + if (p[0] == 1) // Lowest address contains the least significant byte + return true; + else + return false; +} + + + +///b3Select avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360 +///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html +B3_FORCE_INLINE unsigned b3Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) +{ + // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero + // Rely on positive value or'ed with its negative having sign bit on + // and zero value or'ed with its negative (which is still zero) having sign bit off + // Use arithmetic shift right, shifting the sign bit through all 32 bits + unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); + unsigned testEqz = ~testNz; + return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); +} +B3_FORCE_INLINE int b3Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero) +{ + unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); + unsigned testEqz = ~testNz; + return static_cast((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); +} +B3_FORCE_INLINE float b3Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) +{ +#ifdef B3_HAVE_NATIVE_FSEL + return (float)b3Fsel((b3Scalar)condition - b3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); +#else + return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; +#endif +} + +template B3_FORCE_INLINE void b3Swap(T& a, T& b) +{ + T tmp = a; + a = b; + b = tmp; +} + + +//PCK: endian swapping functions +B3_FORCE_INLINE unsigned b3SwapEndian(unsigned val) +{ + return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24)); +} + +B3_FORCE_INLINE unsigned short b3SwapEndian(unsigned short val) +{ + return static_cast(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8)); +} + +B3_FORCE_INLINE unsigned b3SwapEndian(int val) +{ + return b3SwapEndian((unsigned)val); +} + +B3_FORCE_INLINE unsigned short b3SwapEndian(short val) +{ + return b3SwapEndian((unsigned short) val); +} + +///b3SwapFloat uses using char pointers to swap the endianness +////b3SwapFloat/b3SwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values +///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. +///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. +///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. +///so instead of returning a float/double, we return integer/long long integer +B3_FORCE_INLINE unsigned int b3SwapEndianFloat(float d) +{ + unsigned int a = 0; + unsigned char *dst = (unsigned char *)&a; + unsigned char *src = (unsigned char *)&d; + + dst[0] = src[3]; + dst[1] = src[2]; + dst[2] = src[1]; + dst[3] = src[0]; + return a; +} + +// unswap using char pointers +B3_FORCE_INLINE float b3UnswapEndianFloat(unsigned int a) +{ + float d = 0.0f; + unsigned char *src = (unsigned char *)&a; + unsigned char *dst = (unsigned char *)&d; + + dst[0] = src[3]; + dst[1] = src[2]; + dst[2] = src[1]; + dst[3] = src[0]; + + return d; +} + + +// swap using char pointers +B3_FORCE_INLINE void b3SwapEndianDouble(double d, unsigned char* dst) +{ + unsigned char *src = (unsigned char *)&d; + + dst[0] = src[7]; + dst[1] = src[6]; + dst[2] = src[5]; + dst[3] = src[4]; + dst[4] = src[3]; + dst[5] = src[2]; + dst[6] = src[1]; + dst[7] = src[0]; + +} + +// unswap using char pointers +B3_FORCE_INLINE double b3UnswapEndianDouble(const unsigned char *src) +{ + double d = 0.0; + unsigned char *dst = (unsigned char *)&d; + + dst[0] = src[7]; + dst[1] = src[6]; + dst[2] = src[5]; + dst[3] = src[4]; + dst[4] = src[3]; + dst[5] = src[2]; + dst[6] = src[1]; + dst[7] = src[0]; + + return d; +} + +// returns normalized value in range [-B3_PI, B3_PI] +B3_FORCE_INLINE b3Scalar b3NormalizeAngle(b3Scalar angleInRadians) +{ + angleInRadians = b3Fmod(angleInRadians, B3_2_PI); + if(angleInRadians < -B3_PI) + { + return angleInRadians + B3_2_PI; + } + else if(angleInRadians > B3_PI) + { + return angleInRadians - B3_2_PI; + } + else + { + return angleInRadians; + } +} + +///rudimentary class to provide type info +struct b3TypedObject +{ + b3TypedObject(int objectType) + :m_objectType(objectType) + { + } + int m_objectType; + inline int getObjectType() const + { + return m_objectType; + } +}; + + + +///align a pointer to the provided alignment, upwards +template T* b3AlignPointer(T* unalignedPtr, size_t alignment) +{ + + struct b3ConvertPointerSizeT + { + union + { + T* ptr; + size_t integer; + }; + }; + b3ConvertPointerSizeT converter; + + + const size_t bit_mask = ~(alignment - 1); + converter.ptr = unalignedPtr; + converter.integer += alignment-1; + converter.integer &= bit_mask; + return converter.ptr; +} + +#endif //B3_SCALAR_H diff --git a/extern/bullet/src/Bullet3Common/b3StackAlloc.h b/extern/bullet/src/Bullet3Common/b3StackAlloc.h new file mode 100644 index 000000000000..de7de056b507 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3StackAlloc.h @@ -0,0 +1,116 @@ +/* +Copyright (c) 2003-2013 Gino van den Bergen / 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. +*/ + +/* +StackAlloc extracted from GJK-EPA collision solver by Nathanael Presson +Nov.2006 +*/ + +#ifndef B3_STACK_ALLOC +#define B3_STACK_ALLOC + +#include "b3Scalar.h" //for b3Assert +#include "b3AlignedAllocator.h" + +///The b3Block class is an internal structure for the b3StackAlloc memory allocator. +struct b3Block +{ + b3Block* previous; + unsigned char* address; +}; + +///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out) +class b3StackAlloc +{ +public: + + b3StackAlloc(unsigned int size) { ctor();create(size); } + ~b3StackAlloc() { destroy(); } + + inline void create(unsigned int size) + { + destroy(); + data = (unsigned char*) b3AlignedAlloc(size,16); + totalsize = size; + } + inline void destroy() + { + b3Assert(usedsize==0); + //Raise(L"StackAlloc is still in use"); + + if(usedsize==0) + { + if(!ischild && data) + b3AlignedFree(data); + + data = 0; + usedsize = 0; + } + + } + + int getAvailableMemory() const + { + return static_cast(totalsize - usedsize); + } + + unsigned char* allocate(unsigned int size) + { + const unsigned int nus(usedsize+size); + if(nusprevious = current; + pb->address = data+usedsize; + current = pb; + return(pb); + } + B3_FORCE_INLINE void endBlock(b3Block* block) + { + b3Assert(block==current); + //Raise(L"Unmatched blocks"); + if(block==current) + { + current = block->previous; + usedsize = (unsigned int)((block->address-data)-sizeof(b3Block)); + } + } + +private: + void ctor() + { + data = 0; + totalsize = 0; + usedsize = 0; + current = 0; + ischild = false; + } + unsigned char* data; + unsigned int totalsize; + unsigned int usedsize; + b3Block* current; + bool ischild; +}; + +#endif //B3_STACK_ALLOC diff --git a/extern/bullet/src/Bullet3Common/b3Transform.h b/extern/bullet/src/Bullet3Common/b3Transform.h new file mode 100644 index 000000000000..fa480759bebd --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3Transform.h @@ -0,0 +1,304 @@ +/* +Copyright (c) 2003-2013 Gino van den Bergen / 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. +*/ + + +#ifndef B3_TRANSFORM_H +#define B3_TRANSFORM_H + + +#include "b3Matrix3x3.h" + +#ifdef B3_USE_DOUBLE_PRECISION +#define b3TransformData b3TransformDoubleData +#else +#define b3TransformData b3TransformFloatData +#endif + + + + +/**@brief The b3Transform class supports rigid transforms with only translation and rotation and no scaling/shear. + *It can be used in combination with b3Vector3, b3Quaternion and b3Matrix3x3 linear algebra classes. */ +B3_ATTRIBUTE_ALIGNED16(class) b3Transform { + + ///Storage for the rotation + b3Matrix3x3 m_basis; + ///Storage for the translation + b3Vector3 m_origin; + +public: + + /**@brief No initialization constructor */ + b3Transform() {} + /**@brief Constructor from b3Quaternion (optional b3Vector3 ) + * @param q Rotation from quaternion + * @param c Translation from Vector (default 0,0,0) */ + explicit B3_FORCE_INLINE b3Transform(const b3Quaternion& q, + const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0))) + : m_basis(q), + m_origin(c) + {} + + /**@brief Constructor from b3Matrix3x3 (optional b3Vector3) + * @param b Rotation from Matrix + * @param c Translation from Vector default (0,0,0)*/ + explicit B3_FORCE_INLINE b3Transform(const b3Matrix3x3& b, + const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0))) + : m_basis(b), + m_origin(c) + {} + /**@brief Copy constructor */ + B3_FORCE_INLINE b3Transform (const b3Transform& other) + : m_basis(other.m_basis), + m_origin(other.m_origin) + { + } + /**@brief Assignment Operator */ + B3_FORCE_INLINE b3Transform& operator=(const b3Transform& other) + { + m_basis = other.m_basis; + m_origin = other.m_origin; + return *this; + } + + + /**@brief Set the current transform as the value of the product of two transforms + * @param t1 Transform 1 + * @param t2 Transform 2 + * This = Transform1 * Transform2 */ + B3_FORCE_INLINE void mult(const b3Transform& t1, const b3Transform& t2) { + m_basis = t1.m_basis * t2.m_basis; + m_origin = t1(t2.m_origin); + } + +/* void multInverseLeft(const b3Transform& t1, const b3Transform& t2) { + b3Vector3 v = t2.m_origin - t1.m_origin; + m_basis = b3MultTransposeLeft(t1.m_basis, t2.m_basis); + m_origin = v * t1.m_basis; + } + */ + +/**@brief Return the transform of the vector */ + B3_FORCE_INLINE b3Vector3 operator()(const b3Vector3& x) const + { + return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin; + } + + /**@brief Return the transform of the vector */ + B3_FORCE_INLINE b3Vector3 operator*(const b3Vector3& x) const + { + return (*this)(x); + } + + /**@brief Return the transform of the b3Quaternion */ + B3_FORCE_INLINE b3Quaternion operator*(const b3Quaternion& q) const + { + return getRotation() * q; + } + + /**@brief Return the basis matrix for the rotation */ + B3_FORCE_INLINE b3Matrix3x3& getBasis() { return m_basis; } + /**@brief Return the basis matrix for the rotation */ + B3_FORCE_INLINE const b3Matrix3x3& getBasis() const { return m_basis; } + + /**@brief Return the origin vector translation */ + B3_FORCE_INLINE b3Vector3& getOrigin() { return m_origin; } + /**@brief Return the origin vector translation */ + B3_FORCE_INLINE const b3Vector3& getOrigin() const { return m_origin; } + + /**@brief Return a quaternion representing the rotation */ + b3Quaternion getRotation() const { + b3Quaternion q; + m_basis.getRotation(q); + return q; + } + + + /**@brief Set from an array + * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ + void setFromOpenGLMatrix(const b3Scalar *m) + { + m_basis.setFromOpenGLSubMatrix(m); + m_origin.setValue(m[12],m[13],m[14]); + } + + /**@brief Fill an array representation + * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */ + void getOpenGLMatrix(b3Scalar *m) const + { + m_basis.getOpenGLSubMatrix(m); + m[12] = m_origin.getX(); + m[13] = m_origin.getY(); + m[14] = m_origin.getZ(); + m[15] = b3Scalar(1.0); + } + + /**@brief Set the translational element + * @param origin The vector to set the translation to */ + B3_FORCE_INLINE void setOrigin(const b3Vector3& origin) + { + m_origin = origin; + } + + B3_FORCE_INLINE b3Vector3 invXform(const b3Vector3& inVec) const; + + + /**@brief Set the rotational element by b3Matrix3x3 */ + B3_FORCE_INLINE void setBasis(const b3Matrix3x3& basis) + { + m_basis = basis; + } + + /**@brief Set the rotational element by b3Quaternion */ + B3_FORCE_INLINE void setRotation(const b3Quaternion& q) + { + m_basis.setRotation(q); + } + + + /**@brief Set this transformation to the identity */ + void setIdentity() + { + m_basis.setIdentity(); + m_origin.setValue(b3Scalar(0.0), b3Scalar(0.0), b3Scalar(0.0)); + } + + /**@brief Multiply this Transform by another(this = this * another) + * @param t The other transform */ + b3Transform& operator*=(const b3Transform& t) + { + m_origin += m_basis * t.m_origin; + m_basis *= t.m_basis; + return *this; + } + + /**@brief Return the inverse of this transform */ + b3Transform inverse() const + { + b3Matrix3x3 inv = m_basis.transpose(); + return b3Transform(inv, inv * -m_origin); + } + + /**@brief Return the inverse of this transform times the other transform + * @param t The other transform + * return this.inverse() * the other */ + b3Transform inverseTimes(const b3Transform& t) const; + + /**@brief Return the product of this transform and the other */ + b3Transform operator*(const b3Transform& t) const; + + /**@brief Return an identity transform */ + static const b3Transform& getIdentity() + { + static const b3Transform identityTransform(b3Matrix3x3::getIdentity()); + return identityTransform; + } + + void serialize(struct b3TransformData& dataOut) const; + + void serializeFloat(struct b3TransformFloatData& dataOut) const; + + void deSerialize(const struct b3TransformData& dataIn); + + void deSerializeDouble(const struct b3TransformDoubleData& dataIn); + + void deSerializeFloat(const struct b3TransformFloatData& dataIn); + +}; + + +B3_FORCE_INLINE b3Vector3 +b3Transform::invXform(const b3Vector3& inVec) const +{ + b3Vector3 v = inVec - m_origin; + return (m_basis.transpose() * v); +} + +B3_FORCE_INLINE b3Transform +b3Transform::inverseTimes(const b3Transform& t) const +{ + b3Vector3 v = t.getOrigin() - m_origin; + return b3Transform(m_basis.transposeTimes(t.m_basis), + v * m_basis); +} + +B3_FORCE_INLINE b3Transform +b3Transform::operator*(const b3Transform& t) const +{ + return b3Transform(m_basis * t.m_basis, + (*this)(t.m_origin)); +} + +/**@brief Test if two transforms have all elements equal */ +B3_FORCE_INLINE bool operator==(const b3Transform& t1, const b3Transform& t2) +{ + return ( t1.getBasis() == t2.getBasis() && + t1.getOrigin() == t2.getOrigin() ); +} + + +///for serialization +struct b3TransformFloatData +{ + b3Matrix3x3FloatData m_basis; + b3Vector3FloatData m_origin; +}; + +struct b3TransformDoubleData +{ + b3Matrix3x3DoubleData m_basis; + b3Vector3DoubleData m_origin; +}; + + + +B3_FORCE_INLINE void b3Transform::serialize(b3TransformData& dataOut) const +{ + m_basis.serialize(dataOut.m_basis); + m_origin.serialize(dataOut.m_origin); +} + +B3_FORCE_INLINE void b3Transform::serializeFloat(b3TransformFloatData& dataOut) const +{ + m_basis.serializeFloat(dataOut.m_basis); + m_origin.serializeFloat(dataOut.m_origin); +} + + +B3_FORCE_INLINE void b3Transform::deSerialize(const b3TransformData& dataIn) +{ + m_basis.deSerialize(dataIn.m_basis); + m_origin.deSerialize(dataIn.m_origin); +} + +B3_FORCE_INLINE void b3Transform::deSerializeFloat(const b3TransformFloatData& dataIn) +{ + m_basis.deSerializeFloat(dataIn.m_basis); + m_origin.deSerializeFloat(dataIn.m_origin); +} + +B3_FORCE_INLINE void b3Transform::deSerializeDouble(const b3TransformDoubleData& dataIn) +{ + m_basis.deSerializeDouble(dataIn.m_basis); + m_origin.deSerializeDouble(dataIn.m_origin); +} + + +#endif //B3_TRANSFORM_H + + + + + + diff --git a/extern/bullet/src/Bullet3Common/b3TransformUtil.h b/extern/bullet/src/Bullet3Common/b3TransformUtil.h new file mode 100644 index 000000000000..6ce580c13280 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3TransformUtil.h @@ -0,0 +1,228 @@ +/* +Copyright (c) 2003-2013 Gino van den Bergen / 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. +*/ + + +#ifndef B3_TRANSFORM_UTIL_H +#define B3_TRANSFORM_UTIL_H + +#include "b3Transform.h" +#define B3_ANGULAR_MOTION_THRESHOLD b3Scalar(0.5)*B3_HALF_PI + + + + +B3_FORCE_INLINE b3Vector3 b3AabbSupport(const b3Vector3& halfExtents,const b3Vector3& supportDir) +{ + return b3MakeVector3(supportDir.getX() < b3Scalar(0.0) ? -halfExtents.getX() : halfExtents.getX(), + supportDir.getY() < b3Scalar(0.0) ? -halfExtents.getY() : halfExtents.getY(), + supportDir.getZ() < b3Scalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ()); +} + + + + + + +/// Utils related to temporal transforms +class b3TransformUtil +{ + +public: + + static void integrateTransform(const b3Transform& curTrans,const b3Vector3& linvel,const b3Vector3& angvel,b3Scalar timeStep,b3Transform& predictedTransform) + { + predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep); +// #define QUATERNION_DERIVATIVE + #ifdef QUATERNION_DERIVATIVE + b3Quaternion predictedOrn = curTrans.getRotation(); + predictedOrn += (angvel * predictedOrn) * (timeStep * b3Scalar(0.5)); + predictedOrn.normalize(); + #else + //Exponential map + //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia + + b3Vector3 axis; + b3Scalar fAngle = angvel.length(); + //limit the angular motion + if (fAngle*timeStep > B3_ANGULAR_MOTION_THRESHOLD) + { + fAngle = B3_ANGULAR_MOTION_THRESHOLD / timeStep; + } + + if ( fAngle < b3Scalar(0.001) ) + { + // use Taylor's expansions of sync function + axis = angvel*( b3Scalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(b3Scalar(0.020833333333))*fAngle*fAngle ); + } + else + { + // sync(fAngle) = sin(c*fAngle)/t + axis = angvel*( b3Sin(b3Scalar(0.5)*fAngle*timeStep)/fAngle ); + } + b3Quaternion dorn (axis.getX(),axis.getY(),axis.getZ(),b3Cos( fAngle*timeStep*b3Scalar(0.5) )); + b3Quaternion orn0 = curTrans.getRotation(); + + b3Quaternion predictedOrn = dorn * orn0; + predictedOrn.normalize(); + #endif + predictedTransform.setRotation(predictedOrn); + } + + static void calculateVelocityQuaternion(const b3Vector3& pos0,const b3Vector3& pos1,const b3Quaternion& orn0,const b3Quaternion& orn1,b3Scalar timeStep,b3Vector3& linVel,b3Vector3& angVel) + { + linVel = (pos1 - pos0) / timeStep; + b3Vector3 axis; + b3Scalar angle; + if (orn0 != orn1) + { + calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle); + angVel = axis * angle / timeStep; + } else + { + angVel.setValue(0,0,0); + } + } + + static void calculateDiffAxisAngleQuaternion(const b3Quaternion& orn0,const b3Quaternion& orn1a,b3Vector3& axis,b3Scalar& angle) + { + b3Quaternion orn1 = orn0.nearest(orn1a); + b3Quaternion dorn = orn1 * orn0.inverse(); + angle = dorn.getAngle(); + axis = b3MakeVector3(dorn.getX(),dorn.getY(),dorn.getZ()); + axis[3] = b3Scalar(0.); + //check for axis length + b3Scalar len = axis.length2(); + if (len < B3_EPSILON*B3_EPSILON) + axis = b3MakeVector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.)); + else + axis /= b3Sqrt(len); + } + + static void calculateVelocity(const b3Transform& transform0,const b3Transform& transform1,b3Scalar timeStep,b3Vector3& linVel,b3Vector3& angVel) + { + linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep; + b3Vector3 axis; + b3Scalar angle; + calculateDiffAxisAngle(transform0,transform1,axis,angle); + angVel = axis * angle / timeStep; + } + + static void calculateDiffAxisAngle(const b3Transform& transform0,const b3Transform& transform1,b3Vector3& axis,b3Scalar& angle) + { + b3Matrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse(); + b3Quaternion dorn; + dmat.getRotation(dorn); + + ///floating point inaccuracy can lead to w component > 1..., which breaks + dorn.normalize(); + + angle = dorn.getAngle(); + axis = b3MakeVector3(dorn.getX(),dorn.getY(),dorn.getZ()); + axis[3] = b3Scalar(0.); + //check for axis length + b3Scalar len = axis.length2(); + if (len < B3_EPSILON*B3_EPSILON) + axis = b3MakeVector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.)); + else + axis /= b3Sqrt(len); + } + +}; + + +///The b3ConvexSeparatingDistanceUtil can help speed up convex collision detection +///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance +class b3ConvexSeparatingDistanceUtil +{ + b3Quaternion m_ornA; + b3Quaternion m_ornB; + b3Vector3 m_posA; + b3Vector3 m_posB; + + b3Vector3 m_separatingNormal; + + b3Scalar m_boundingRadiusA; + b3Scalar m_boundingRadiusB; + b3Scalar m_separatingDistance; + +public: + + b3ConvexSeparatingDistanceUtil(b3Scalar boundingRadiusA,b3Scalar boundingRadiusB) + :m_boundingRadiusA(boundingRadiusA), + m_boundingRadiusB(boundingRadiusB), + m_separatingDistance(0.f) + { + } + + b3Scalar getConservativeSeparatingDistance() + { + return m_separatingDistance; + } + + void updateSeparatingDistance(const b3Transform& transA,const b3Transform& transB) + { + const b3Vector3& toPosA = transA.getOrigin(); + const b3Vector3& toPosB = transB.getOrigin(); + b3Quaternion toOrnA = transA.getRotation(); + b3Quaternion toOrnB = transB.getRotation(); + + if (m_separatingDistance>0.f) + { + + + b3Vector3 linVelA,angVelA,linVelB,angVelB; + b3TransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,b3Scalar(1.),linVelA,angVelA); + b3TransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,b3Scalar(1.),linVelB,angVelB); + b3Scalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB; + b3Vector3 relLinVel = (linVelB-linVelA); + b3Scalar relLinVelocLength = relLinVel.dot(m_separatingNormal); + if (relLinVelocLength<0.f) + { + relLinVelocLength = 0.f; + } + + b3Scalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength; + m_separatingDistance -= projectedMotion; + } + + m_posA = toPosA; + m_posB = toPosB; + m_ornA = toOrnA; + m_ornB = toOrnB; + } + + void initSeparatingDistance(const b3Vector3& separatingVector,b3Scalar separatingDistance,const b3Transform& transA,const b3Transform& transB) + { + m_separatingDistance = separatingDistance; + + if (m_separatingDistance>0.f) + { + m_separatingNormal = separatingVector; + + const b3Vector3& toPosA = transA.getOrigin(); + const b3Vector3& toPosB = transB.getOrigin(); + b3Quaternion toOrnA = transA.getRotation(); + b3Quaternion toOrnB = transB.getRotation(); + m_posA = toPosA; + m_posB = toPosB; + m_ornA = toOrnA; + m_ornB = toOrnB; + } + } + +}; + + +#endif //B3_TRANSFORM_UTIL_H + diff --git a/extern/bullet/src/Bullet3Common/b3Vector3.cpp b/extern/bullet/src/Bullet3Common/b3Vector3.cpp new file mode 100644 index 000000000000..5f5ac4ac0435 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3Vector3.cpp @@ -0,0 +1,1631 @@ +/* + Copyright (c) 2011-213 Apple Inc. 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 source version has been altered. + */ + +#if defined (_WIN32) || defined (__i386__) +#define B3_USE_SSE_IN_API +#endif + +#include "b3Vector3.h" + +#if defined (B3_USE_SSE) || defined (B3_USE_NEON) + +#ifdef __APPLE__ +#include +typedef float float4 __attribute__ ((vector_size(16))); +#else +#define float4 __m128 +#endif +//typedef uint32_t uint4 __attribute__ ((vector_size(16))); + + +#if defined B3_USE_SSE || defined _WIN32 + +#define LOG2_ARRAY_SIZE 6 +#define STACK_ARRAY_COUNT (1UL << LOG2_ARRAY_SIZE) + +#include + +long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, float *dotResult ); +long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, float *dotResult ) +{ + const float4 *vertices = (const float4*) vv; + static const unsigned char indexTable[16] = {(unsigned char)-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 }; + float4 dotMax = b3Assign128( -B3_INFINITY, -B3_INFINITY, -B3_INFINITY, -B3_INFINITY ); + float4 vvec = _mm_loadu_ps( vec ); + float4 vHi = b3CastiTo128f(_mm_shuffle_epi32( b3CastfTo128i( vvec), 0xaa )); /// zzzz + float4 vLo = _mm_movelh_ps( vvec, vvec ); /// xyxy + + long maxIndex = -1L; + + size_t segment = 0; + float4 stack_array[ STACK_ARRAY_COUNT ]; + +#if DEBUG + // memset( stack_array, -1, STACK_ARRAY_COUNT * sizeof(stack_array[0]) ); +#endif + + size_t index; + float4 max; + // Faster loop without cleanup code for full tiles + for ( segment = 0; segment + STACK_ARRAY_COUNT*4 <= count; segment += STACK_ARRAY_COUNT*4 ) + { + max = dotMax; + + for( index = 0; index < STACK_ARRAY_COUNT; index+= 4 ) + { // do four dot products at a time. Carefully avoid touching the w element. + float4 v0 = vertices[0]; + float4 v1 = vertices[1]; + float4 v2 = vertices[2]; + float4 v3 = vertices[3]; vertices += 4; + + float4 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 + float4 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 + float4 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 + float4 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 + + lo0 = lo0*vLo; + lo1 = lo1*vLo; + float4 z = _mm_shuffle_ps(hi0, hi1, 0x88); + float4 x = _mm_shuffle_ps(lo0, lo1, 0x88); + float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd); + z = z*vHi; + x = x+y; + x = x+z; + stack_array[index] = x; + max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan + + v0 = vertices[0]; + v1 = vertices[1]; + v2 = vertices[2]; + v3 = vertices[3]; vertices += 4; + + lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 + hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 + lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 + hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 + + lo0 = lo0*vLo; + lo1 = lo1*vLo; + z = _mm_shuffle_ps(hi0, hi1, 0x88); + x = _mm_shuffle_ps(lo0, lo1, 0x88); + y = _mm_shuffle_ps(lo0, lo1, 0xdd); + z = z*vHi; + x = x+y; + x = x+z; + stack_array[index+1] = x; + max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan + + v0 = vertices[0]; + v1 = vertices[1]; + v2 = vertices[2]; + v3 = vertices[3]; vertices += 4; + + lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 + hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 + lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 + hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 + + lo0 = lo0*vLo; + lo1 = lo1*vLo; + z = _mm_shuffle_ps(hi0, hi1, 0x88); + x = _mm_shuffle_ps(lo0, lo1, 0x88); + y = _mm_shuffle_ps(lo0, lo1, 0xdd); + z = z*vHi; + x = x+y; + x = x+z; + stack_array[index+2] = x; + max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan + + v0 = vertices[0]; + v1 = vertices[1]; + v2 = vertices[2]; + v3 = vertices[3]; vertices += 4; + + lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 + hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 + lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 + hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 + + lo0 = lo0*vLo; + lo1 = lo1*vLo; + z = _mm_shuffle_ps(hi0, hi1, 0x88); + x = _mm_shuffle_ps(lo0, lo1, 0x88); + y = _mm_shuffle_ps(lo0, lo1, 0xdd); + z = z*vHi; + x = x+y; + x = x+z; + stack_array[index+3] = x; + max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan + + // It is too costly to keep the index of the max here. We will look for it again later. We save a lot of work this way. + } + + // If we found a new max + if( 0xf != _mm_movemask_ps( (float4) _mm_cmpeq_ps(max, dotMax))) + { + // copy the new max across all lanes of our max accumulator + max = _mm_max_ps(max, (float4) _mm_shuffle_ps( max, max, 0x4e)); + max = _mm_max_ps(max, (float4) _mm_shuffle_ps( max, max, 0xb1)); + + dotMax = max; + + // find first occurrence of that max + size_t test; + for( index = 0; 0 == (test=_mm_movemask_ps( _mm_cmpeq_ps( stack_array[index], max))); index++ ) // local_count must be a multiple of 4 + {} + // record where it is. + maxIndex = 4*index + segment + indexTable[test]; + } + } + + // account for work we've already done + count -= segment; + + // Deal with the last < STACK_ARRAY_COUNT vectors + max = dotMax; + index = 0; + + + if( b3Unlikely( count > 16) ) + { + for( ; index + 4 <= count / 4; index+=4 ) + { // do four dot products at a time. Carefully avoid touching the w element. + float4 v0 = vertices[0]; + float4 v1 = vertices[1]; + float4 v2 = vertices[2]; + float4 v3 = vertices[3]; vertices += 4; + + float4 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 + float4 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 + float4 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 + float4 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 + + lo0 = lo0*vLo; + lo1 = lo1*vLo; + float4 z = _mm_shuffle_ps(hi0, hi1, 0x88); + float4 x = _mm_shuffle_ps(lo0, lo1, 0x88); + float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd); + z = z*vHi; + x = x+y; + x = x+z; + stack_array[index] = x; + max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan + + v0 = vertices[0]; + v1 = vertices[1]; + v2 = vertices[2]; + v3 = vertices[3]; vertices += 4; + + lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 + hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 + lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 + hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 + + lo0 = lo0*vLo; + lo1 = lo1*vLo; + z = _mm_shuffle_ps(hi0, hi1, 0x88); + x = _mm_shuffle_ps(lo0, lo1, 0x88); + y = _mm_shuffle_ps(lo0, lo1, 0xdd); + z = z*vHi; + x = x+y; + x = x+z; + stack_array[index+1] = x; + max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan + + v0 = vertices[0]; + v1 = vertices[1]; + v2 = vertices[2]; + v3 = vertices[3]; vertices += 4; + + lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 + hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 + lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 + hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 + + lo0 = lo0*vLo; + lo1 = lo1*vLo; + z = _mm_shuffle_ps(hi0, hi1, 0x88); + x = _mm_shuffle_ps(lo0, lo1, 0x88); + y = _mm_shuffle_ps(lo0, lo1, 0xdd); + z = z*vHi; + x = x+y; + x = x+z; + stack_array[index+2] = x; + max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan + + v0 = vertices[0]; + v1 = vertices[1]; + v2 = vertices[2]; + v3 = vertices[3]; vertices += 4; + + lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 + hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 + lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 + hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 + + lo0 = lo0*vLo; + lo1 = lo1*vLo; + z = _mm_shuffle_ps(hi0, hi1, 0x88); + x = _mm_shuffle_ps(lo0, lo1, 0x88); + y = _mm_shuffle_ps(lo0, lo1, 0xdd); + z = z*vHi; + x = x+y; + x = x+z; + stack_array[index+3] = x; + max = _mm_max_ps( x, max ); // control the order here so that max is never NaN even if x is nan + + // It is too costly to keep the index of the max here. We will look for it again later. We save a lot of work this way. + } + } + + size_t localCount = (count & -4L) - 4*index; + if( localCount ) + { +#ifdef __APPLE__ + float4 t0, t1, t2, t3, t4; + float4 * sap = &stack_array[index + localCount / 4]; + vertices += localCount; // counter the offset + size_t byteIndex = -(localCount) * sizeof(float); + //AT&T Code style assembly + asm volatile + ( ".align 4 \n\ + 0: movaps %[max], %[t2] // move max out of the way to avoid propagating NaNs in max \n\ + movaps (%[vertices], %[byteIndex], 4), %[t0] // vertices[0] \n\ + movaps 16(%[vertices], %[byteIndex], 4), %[t1] // vertices[1] \n\ + movaps %[t0], %[max] // vertices[0] \n\ + movlhps %[t1], %[max] // x0y0x1y1 \n\ + movaps 32(%[vertices], %[byteIndex], 4), %[t3] // vertices[2] \n\ + movaps 48(%[vertices], %[byteIndex], 4), %[t4] // vertices[3] \n\ + mulps %[vLo], %[max] // x0y0x1y1 * vLo \n\ + movhlps %[t0], %[t1] // z0w0z1w1 \n\ + movaps %[t3], %[t0] // vertices[2] \n\ + movlhps %[t4], %[t0] // x2y2x3y3 \n\ + mulps %[vLo], %[t0] // x2y2x3y3 * vLo \n\ + movhlps %[t3], %[t4] // z2w2z3w3 \n\ + shufps $0x88, %[t4], %[t1] // z0z1z2z3 \n\ + mulps %[vHi], %[t1] // z0z1z2z3 * vHi \n\ + movaps %[max], %[t3] // x0y0x1y1 * vLo \n\ + shufps $0x88, %[t0], %[max] // x0x1x2x3 * vLo.x \n\ + shufps $0xdd, %[t0], %[t3] // y0y1y2y3 * vLo.y \n\ + addps %[t3], %[max] // x + y \n\ + addps %[t1], %[max] // x + y + z \n\ + movaps %[max], (%[sap], %[byteIndex]) // record result for later scrutiny \n\ + maxps %[t2], %[max] // record max, restore max \n\ + add $16, %[byteIndex] // advance loop counter\n\ + jnz 0b \n\ + " + : [max] "+x" (max), [t0] "=&x" (t0), [t1] "=&x" (t1), [t2] "=&x" (t2), [t3] "=&x" (t3), [t4] "=&x" (t4), [byteIndex] "+r" (byteIndex) + : [vLo] "x" (vLo), [vHi] "x" (vHi), [vertices] "r" (vertices), [sap] "r" (sap) + : "memory", "cc" + ); + index += localCount/4; +#else + { + for( unsigned int i=0; i 16) ) + { + for( ; index + 4 <= count / 4; index+=4 ) + { // do four dot products at a time. Carefully avoid touching the w element. + float4 v0 = vertices[0]; + float4 v1 = vertices[1]; + float4 v2 = vertices[2]; + float4 v3 = vertices[3]; vertices += 4; + + float4 lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 + float4 hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 + float4 lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 + float4 hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 + + lo0 = lo0*vLo; + lo1 = lo1*vLo; + float4 z = _mm_shuffle_ps(hi0, hi1, 0x88); + float4 x = _mm_shuffle_ps(lo0, lo1, 0x88); + float4 y = _mm_shuffle_ps(lo0, lo1, 0xdd); + z = z*vHi; + x = x+y; + x = x+z; + stack_array[index] = x; + min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan + + v0 = vertices[0]; + v1 = vertices[1]; + v2 = vertices[2]; + v3 = vertices[3]; vertices += 4; + + lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 + hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 + lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 + hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 + + lo0 = lo0*vLo; + lo1 = lo1*vLo; + z = _mm_shuffle_ps(hi0, hi1, 0x88); + x = _mm_shuffle_ps(lo0, lo1, 0x88); + y = _mm_shuffle_ps(lo0, lo1, 0xdd); + z = z*vHi; + x = x+y; + x = x+z; + stack_array[index+1] = x; + min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan + + v0 = vertices[0]; + v1 = vertices[1]; + v2 = vertices[2]; + v3 = vertices[3]; vertices += 4; + + lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 + hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 + lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 + hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 + + lo0 = lo0*vLo; + lo1 = lo1*vLo; + z = _mm_shuffle_ps(hi0, hi1, 0x88); + x = _mm_shuffle_ps(lo0, lo1, 0x88); + y = _mm_shuffle_ps(lo0, lo1, 0xdd); + z = z*vHi; + x = x+y; + x = x+z; + stack_array[index+2] = x; + min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan + + v0 = vertices[0]; + v1 = vertices[1]; + v2 = vertices[2]; + v3 = vertices[3]; vertices += 4; + + lo0 = _mm_movelh_ps( v0, v1); // x0y0x1y1 + hi0 = _mm_movehl_ps( v1, v0); // z0?0z1?1 + lo1 = _mm_movelh_ps( v2, v3); // x2y2x3y3 + hi1 = _mm_movehl_ps( v3, v2); // z2?2z3?3 + + lo0 = lo0*vLo; + lo1 = lo1*vLo; + z = _mm_shuffle_ps(hi0, hi1, 0x88); + x = _mm_shuffle_ps(lo0, lo1, 0x88); + y = _mm_shuffle_ps(lo0, lo1, 0xdd); + z = z*vHi; + x = x+y; + x = x+z; + stack_array[index+3] = x; + min = _mm_min_ps( x, min ); // control the order here so that min is never NaN even if x is nan + + // It is too costly to keep the index of the min here. We will look for it again later. We save a lot of work this way. + } + } + + size_t localCount = (count & -4L) - 4*index; + if( localCount ) + { + + +#ifdef __APPLE__ + vertices += localCount; // counter the offset + float4 t0, t1, t2, t3, t4; + size_t byteIndex = -(localCount) * sizeof(float); + float4 * sap = &stack_array[index + localCount / 4]; + + asm volatile + ( ".align 4 \n\ + 0: movaps %[min], %[t2] // move min out of the way to avoid propagating NaNs in min \n\ + movaps (%[vertices], %[byteIndex], 4), %[t0] // vertices[0] \n\ + movaps 16(%[vertices], %[byteIndex], 4), %[t1] // vertices[1] \n\ + movaps %[t0], %[min] // vertices[0] \n\ + movlhps %[t1], %[min] // x0y0x1y1 \n\ + movaps 32(%[vertices], %[byteIndex], 4), %[t3] // vertices[2] \n\ + movaps 48(%[vertices], %[byteIndex], 4), %[t4] // vertices[3] \n\ + mulps %[vLo], %[min] // x0y0x1y1 * vLo \n\ + movhlps %[t0], %[t1] // z0w0z1w1 \n\ + movaps %[t3], %[t0] // vertices[2] \n\ + movlhps %[t4], %[t0] // x2y2x3y3 \n\ + movhlps %[t3], %[t4] // z2w2z3w3 \n\ + mulps %[vLo], %[t0] // x2y2x3y3 * vLo \n\ + shufps $0x88, %[t4], %[t1] // z0z1z2z3 \n\ + mulps %[vHi], %[t1] // z0z1z2z3 * vHi \n\ + movaps %[min], %[t3] // x0y0x1y1 * vLo \n\ + shufps $0x88, %[t0], %[min] // x0x1x2x3 * vLo.x \n\ + shufps $0xdd, %[t0], %[t3] // y0y1y2y3 * vLo.y \n\ + addps %[t3], %[min] // x + y \n\ + addps %[t1], %[min] // x + y + z \n\ + movaps %[min], (%[sap], %[byteIndex]) // record result for later scrutiny \n\ + minps %[t2], %[min] // record min, restore min \n\ + add $16, %[byteIndex] // advance loop counter\n\ + jnz 0b \n\ + " + : [min] "+x" (min), [t0] "=&x" (t0), [t1] "=&x" (t1), [t2] "=&x" (t2), [t3] "=&x" (t3), [t4] "=&x" (t4), [byteIndex] "+r" (byteIndex) + : [vLo] "x" (vLo), [vHi] "x" (vHi), [vertices] "r" (vertices), [sap] "r" (sap) + : "memory", "cc" + ); + index += localCount/4; +#else + { + for( unsigned int i=0; i + + +static long b3_maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult ); +static long b3_maxdot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult ); +static long b3_maxdot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult ); +static long b3_mindot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult ); +static long b3_mindot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult ); +static long b3_mindot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult ); + +long (*b3_maxdot_large)( const float *vv, const float *vec, unsigned long count, float *dotResult ) = b3_maxdot_large_sel; +long (*b3_mindot_large)( const float *vv, const float *vec, unsigned long count, float *dotResult ) = b3_mindot_large_sel; + +extern "C" {int _get_cpu_capabilities( void );} + +static long b3_maxdot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult ) +{ + if( _get_cpu_capabilities() & 0x2000 ) + b3_maxdot_large = _maxdot_large_v1; + else + b3_maxdot_large = _maxdot_large_v0; + + return b3_maxdot_large(vv, vec, count, dotResult); +} + +static long b3_mindot_large_sel( const float *vv, const float *vec, unsigned long count, float *dotResult ) +{ + if( _get_cpu_capabilities() & 0x2000 ) + b3_mindot_large = _mindot_large_v1; + else + b3_mindot_large = _mindot_large_v0; + + return b3_mindot_large(vv, vec, count, dotResult); +} + + + +#define vld1q_f32_aligned_postincrement( _ptr ) ({ float32x4_t _r; asm( "vld1.f32 {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; }) + + +long b3_maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult ) +{ + unsigned long i = 0; + float32x4_t vvec = vld1q_f32_aligned_postincrement( vec ); + float32x2_t vLo = vget_low_f32(vvec); + float32x2_t vHi = vdup_lane_f32(vget_high_f32(vvec), 0); + float32x2_t dotMaxLo = (float32x2_t) { -B3_INFINITY, -B3_INFINITY }; + float32x2_t dotMaxHi = (float32x2_t) { -B3_INFINITY, -B3_INFINITY }; + uint32x2_t indexLo = (uint32x2_t) {0, 1}; + uint32x2_t indexHi = (uint32x2_t) {2, 3}; + uint32x2_t iLo = (uint32x2_t) {-1, -1}; + uint32x2_t iHi = (uint32x2_t) {-1, -1}; + const uint32x2_t four = (uint32x2_t) {4,4}; + + for( ; i+8 <= count; i+= 8 ) + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v2 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v3 = vld1q_f32_aligned_postincrement( vv ); + + float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo); + float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo); + float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo); + float32x2_t xy3 = vmul_f32( vget_low_f32(v3), vLo); + + float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x2x2_t z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3)); + float32x2_t zLo = vmul_f32( z0.val[0], vHi); + float32x2_t zHi = vmul_f32( z1.val[0], vHi); + + float32x2_t rLo = vpadd_f32( xy0, xy1); + float32x2_t rHi = vpadd_f32( xy2, xy3); + rLo = vadd_f32(rLo, zLo); + rHi = vadd_f32(rHi, zHi); + + uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo ); + uint32x2_t maskHi = vcgt_f32( rHi, dotMaxHi ); + dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo); + dotMaxHi = vbsl_f32( maskHi, rHi, dotMaxHi); + iLo = vbsl_u32(maskLo, indexLo, iLo); + iHi = vbsl_u32(maskHi, indexHi, iHi); + indexLo = vadd_u32(indexLo, four); + indexHi = vadd_u32(indexHi, four); + + v0 = vld1q_f32_aligned_postincrement( vv ); + v1 = vld1q_f32_aligned_postincrement( vv ); + v2 = vld1q_f32_aligned_postincrement( vv ); + v3 = vld1q_f32_aligned_postincrement( vv ); + + xy0 = vmul_f32( vget_low_f32(v0), vLo); + xy1 = vmul_f32( vget_low_f32(v1), vLo); + xy2 = vmul_f32( vget_low_f32(v2), vLo); + xy3 = vmul_f32( vget_low_f32(v3), vLo); + + z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1)); + z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3)); + zLo = vmul_f32( z0.val[0], vHi); + zHi = vmul_f32( z1.val[0], vHi); + + rLo = vpadd_f32( xy0, xy1); + rHi = vpadd_f32( xy2, xy3); + rLo = vadd_f32(rLo, zLo); + rHi = vadd_f32(rHi, zHi); + + maskLo = vcgt_f32( rLo, dotMaxLo ); + maskHi = vcgt_f32( rHi, dotMaxHi ); + dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo); + dotMaxHi = vbsl_f32( maskHi, rHi, dotMaxHi); + iLo = vbsl_u32(maskLo, indexLo, iLo); + iHi = vbsl_u32(maskHi, indexHi, iHi); + indexLo = vadd_u32(indexLo, four); + indexHi = vadd_u32(indexHi, four); + } + + for( ; i+4 <= count; i+= 4 ) + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v2 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v3 = vld1q_f32_aligned_postincrement( vv ); + + float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo); + float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo); + float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo); + float32x2_t xy3 = vmul_f32( vget_low_f32(v3), vLo); + + float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x2x2_t z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3)); + float32x2_t zLo = vmul_f32( z0.val[0], vHi); + float32x2_t zHi = vmul_f32( z1.val[0], vHi); + + float32x2_t rLo = vpadd_f32( xy0, xy1); + float32x2_t rHi = vpadd_f32( xy2, xy3); + rLo = vadd_f32(rLo, zLo); + rHi = vadd_f32(rHi, zHi); + + uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo ); + uint32x2_t maskHi = vcgt_f32( rHi, dotMaxHi ); + dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo); + dotMaxHi = vbsl_f32( maskHi, rHi, dotMaxHi); + iLo = vbsl_u32(maskLo, indexLo, iLo); + iHi = vbsl_u32(maskHi, indexHi, iHi); + indexLo = vadd_u32(indexLo, four); + indexHi = vadd_u32(indexHi, four); + } + + switch( count & 3 ) + { + case 3: + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v2 = vld1q_f32_aligned_postincrement( vv ); + + float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo); + float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo); + float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo); + + float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x2_t zLo = vmul_f32( z0.val[0], vHi); + float32x2_t zHi = vmul_f32( vdup_lane_f32(vget_high_f32(v2), 0), vHi); + + float32x2_t rLo = vpadd_f32( xy0, xy1); + float32x2_t rHi = vpadd_f32( xy2, xy2); + rLo = vadd_f32(rLo, zLo); + rHi = vadd_f32(rHi, zHi); + + uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo ); + uint32x2_t maskHi = vcgt_f32( rHi, dotMaxHi ); + dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo); + dotMaxHi = vbsl_f32( maskHi, rHi, dotMaxHi); + iLo = vbsl_u32(maskLo, indexLo, iLo); + iHi = vbsl_u32(maskHi, indexHi, iHi); + } + break; + case 2: + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + + float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo); + float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo); + + float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x2_t zLo = vmul_f32( z0.val[0], vHi); + + float32x2_t rLo = vpadd_f32( xy0, xy1); + rLo = vadd_f32(rLo, zLo); + + uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo ); + dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo); + iLo = vbsl_u32(maskLo, indexLo, iLo); + } + break; + case 1: + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo); + float32x2_t z0 = vdup_lane_f32(vget_high_f32(v0), 0); + float32x2_t zLo = vmul_f32( z0, vHi); + float32x2_t rLo = vpadd_f32( xy0, xy0); + rLo = vadd_f32(rLo, zLo); + uint32x2_t maskLo = vcgt_f32( rLo, dotMaxLo ); + dotMaxLo = vbsl_f32( maskLo, rLo, dotMaxLo); + iLo = vbsl_u32(maskLo, indexLo, iLo); + } + break; + + default: + break; + } + + // select best answer between hi and lo results + uint32x2_t mask = vcgt_f32( dotMaxHi, dotMaxLo ); + dotMaxLo = vbsl_f32(mask, dotMaxHi, dotMaxLo); + iLo = vbsl_u32(mask, iHi, iLo); + + // select best answer between even and odd results + dotMaxHi = vdup_lane_f32(dotMaxLo, 1); + iHi = vdup_lane_u32(iLo, 1); + mask = vcgt_f32( dotMaxHi, dotMaxLo ); + dotMaxLo = vbsl_f32(mask, dotMaxHi, dotMaxLo); + iLo = vbsl_u32(mask, iHi, iLo); + + *dotResult = vget_lane_f32( dotMaxLo, 0); + return vget_lane_u32(iLo, 0); +} + + +long b3_maxdot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult ) +{ + float32x4_t vvec = vld1q_f32_aligned_postincrement( vec ); + float32x4_t vLo = vcombine_f32(vget_low_f32(vvec), vget_low_f32(vvec)); + float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0); + const uint32x4_t four = (uint32x4_t){ 4, 4, 4, 4 }; + uint32x4_t local_index = (uint32x4_t) {0, 1, 2, 3}; + uint32x4_t index = (uint32x4_t) { -1, -1, -1, -1 }; + float32x4_t maxDot = (float32x4_t) { -B3_INFINITY, -B3_INFINITY, -B3_INFINITY, -B3_INFINITY }; + + unsigned long i = 0; + for( ; i + 8 <= count; i += 8 ) + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v2 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v3 = vld1q_f32_aligned_postincrement( vv ); + + // the next two lines should resolve to a single vswp d, d + float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1)); + float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3)); + // the next two lines should resolve to a single vswp d, d + float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3)); + + xy0 = vmulq_f32(xy0, vLo); + xy1 = vmulq_f32(xy1, vLo); + + float32x4x2_t zb = vuzpq_f32( z0, z1); + float32x4_t z = vmulq_f32( zb.val[0], vHi); + float32x4x2_t xy = vuzpq_f32( xy0, xy1); + float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]); + x = vaddq_f32(x, z); + + uint32x4_t mask = vcgtq_f32(x, maxDot); + maxDot = vbslq_f32( mask, x, maxDot); + index = vbslq_u32(mask, local_index, index); + local_index = vaddq_u32(local_index, four); + + v0 = vld1q_f32_aligned_postincrement( vv ); + v1 = vld1q_f32_aligned_postincrement( vv ); + v2 = vld1q_f32_aligned_postincrement( vv ); + v3 = vld1q_f32_aligned_postincrement( vv ); + + // the next two lines should resolve to a single vswp d, d + xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1)); + xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3)); + // the next two lines should resolve to a single vswp d, d + z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1)); + z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3)); + + xy0 = vmulq_f32(xy0, vLo); + xy1 = vmulq_f32(xy1, vLo); + + zb = vuzpq_f32( z0, z1); + z = vmulq_f32( zb.val[0], vHi); + xy = vuzpq_f32( xy0, xy1); + x = vaddq_f32(xy.val[0], xy.val[1]); + x = vaddq_f32(x, z); + + mask = vcgtq_f32(x, maxDot); + maxDot = vbslq_f32( mask, x, maxDot); + index = vbslq_u32(mask, local_index, index); + local_index = vaddq_u32(local_index, four); + } + + for( ; i + 4 <= count; i += 4 ) + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v2 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v3 = vld1q_f32_aligned_postincrement( vv ); + + // the next two lines should resolve to a single vswp d, d + float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1)); + float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3)); + // the next two lines should resolve to a single vswp d, d + float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3)); + + xy0 = vmulq_f32(xy0, vLo); + xy1 = vmulq_f32(xy1, vLo); + + float32x4x2_t zb = vuzpq_f32( z0, z1); + float32x4_t z = vmulq_f32( zb.val[0], vHi); + float32x4x2_t xy = vuzpq_f32( xy0, xy1); + float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]); + x = vaddq_f32(x, z); + + uint32x4_t mask = vcgtq_f32(x, maxDot); + maxDot = vbslq_f32( mask, x, maxDot); + index = vbslq_u32(mask, local_index, index); + local_index = vaddq_u32(local_index, four); + } + + switch (count & 3) { + case 3: + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v2 = vld1q_f32_aligned_postincrement( vv ); + + // the next two lines should resolve to a single vswp d, d + float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1)); + float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v2)); + // the next two lines should resolve to a single vswp d, d + float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v2)); + + xy0 = vmulq_f32(xy0, vLo); + xy1 = vmulq_f32(xy1, vLo); + + float32x4x2_t zb = vuzpq_f32( z0, z1); + float32x4_t z = vmulq_f32( zb.val[0], vHi); + float32x4x2_t xy = vuzpq_f32( xy0, xy1); + float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]); + x = vaddq_f32(x, z); + + uint32x4_t mask = vcgtq_f32(x, maxDot); + maxDot = vbslq_f32( mask, x, maxDot); + index = vbslq_u32(mask, local_index, index); + local_index = vaddq_u32(local_index, four); + } + break; + + case 2: + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + + // the next two lines should resolve to a single vswp d, d + float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1)); + // the next two lines should resolve to a single vswp d, d + float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1)); + + xy0 = vmulq_f32(xy0, vLo); + + float32x4x2_t zb = vuzpq_f32( z0, z0); + float32x4_t z = vmulq_f32( zb.val[0], vHi); + float32x4x2_t xy = vuzpq_f32( xy0, xy0); + float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]); + x = vaddq_f32(x, z); + + uint32x4_t mask = vcgtq_f32(x, maxDot); + maxDot = vbslq_f32( mask, x, maxDot); + index = vbslq_u32(mask, local_index, index); + local_index = vaddq_u32(local_index, four); + } + break; + + case 1: + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + + // the next two lines should resolve to a single vswp d, d + float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v0)); + // the next two lines should resolve to a single vswp d, d + float32x4_t z = vdupq_lane_f32(vget_high_f32(v0), 0); + + xy0 = vmulq_f32(xy0, vLo); + + z = vmulq_f32( z, vHi); + float32x4x2_t xy = vuzpq_f32( xy0, xy0); + float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]); + x = vaddq_f32(x, z); + + uint32x4_t mask = vcgtq_f32(x, maxDot); + maxDot = vbslq_f32( mask, x, maxDot); + index = vbslq_u32(mask, local_index, index); + local_index = vaddq_u32(local_index, four); + } + break; + + default: + break; + } + + + // select best answer between hi and lo results + uint32x2_t mask = vcgt_f32( vget_high_f32(maxDot), vget_low_f32(maxDot)); + float32x2_t maxDot2 = vbsl_f32(mask, vget_high_f32(maxDot), vget_low_f32(maxDot)); + uint32x2_t index2 = vbsl_u32(mask, vget_high_u32(index), vget_low_u32(index)); + + // select best answer between even and odd results + float32x2_t maxDotO = vdup_lane_f32(maxDot2, 1); + uint32x2_t indexHi = vdup_lane_u32(index2, 1); + mask = vcgt_f32( maxDotO, maxDot2 ); + maxDot2 = vbsl_f32(mask, maxDotO, maxDot2); + index2 = vbsl_u32(mask, indexHi, index2); + + *dotResult = vget_lane_f32( maxDot2, 0); + return vget_lane_u32(index2, 0); + +} + +long b3_mindot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult ) +{ + unsigned long i = 0; + float32x4_t vvec = vld1q_f32_aligned_postincrement( vec ); + float32x2_t vLo = vget_low_f32(vvec); + float32x2_t vHi = vdup_lane_f32(vget_high_f32(vvec), 0); + float32x2_t dotMinLo = (float32x2_t) { B3_INFINITY, B3_INFINITY }; + float32x2_t dotMinHi = (float32x2_t) { B3_INFINITY, B3_INFINITY }; + uint32x2_t indexLo = (uint32x2_t) {0, 1}; + uint32x2_t indexHi = (uint32x2_t) {2, 3}; + uint32x2_t iLo = (uint32x2_t) {-1, -1}; + uint32x2_t iHi = (uint32x2_t) {-1, -1}; + const uint32x2_t four = (uint32x2_t) {4,4}; + + for( ; i+8 <= count; i+= 8 ) + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v2 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v3 = vld1q_f32_aligned_postincrement( vv ); + + float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo); + float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo); + float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo); + float32x2_t xy3 = vmul_f32( vget_low_f32(v3), vLo); + + float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x2x2_t z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3)); + float32x2_t zLo = vmul_f32( z0.val[0], vHi); + float32x2_t zHi = vmul_f32( z1.val[0], vHi); + + float32x2_t rLo = vpadd_f32( xy0, xy1); + float32x2_t rHi = vpadd_f32( xy2, xy3); + rLo = vadd_f32(rLo, zLo); + rHi = vadd_f32(rHi, zHi); + + uint32x2_t maskLo = vclt_f32( rLo, dotMinLo ); + uint32x2_t maskHi = vclt_f32( rHi, dotMinHi ); + dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo); + dotMinHi = vbsl_f32( maskHi, rHi, dotMinHi); + iLo = vbsl_u32(maskLo, indexLo, iLo); + iHi = vbsl_u32(maskHi, indexHi, iHi); + indexLo = vadd_u32(indexLo, four); + indexHi = vadd_u32(indexHi, four); + + v0 = vld1q_f32_aligned_postincrement( vv ); + v1 = vld1q_f32_aligned_postincrement( vv ); + v2 = vld1q_f32_aligned_postincrement( vv ); + v3 = vld1q_f32_aligned_postincrement( vv ); + + xy0 = vmul_f32( vget_low_f32(v0), vLo); + xy1 = vmul_f32( vget_low_f32(v1), vLo); + xy2 = vmul_f32( vget_low_f32(v2), vLo); + xy3 = vmul_f32( vget_low_f32(v3), vLo); + + z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1)); + z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3)); + zLo = vmul_f32( z0.val[0], vHi); + zHi = vmul_f32( z1.val[0], vHi); + + rLo = vpadd_f32( xy0, xy1); + rHi = vpadd_f32( xy2, xy3); + rLo = vadd_f32(rLo, zLo); + rHi = vadd_f32(rHi, zHi); + + maskLo = vclt_f32( rLo, dotMinLo ); + maskHi = vclt_f32( rHi, dotMinHi ); + dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo); + dotMinHi = vbsl_f32( maskHi, rHi, dotMinHi); + iLo = vbsl_u32(maskLo, indexLo, iLo); + iHi = vbsl_u32(maskHi, indexHi, iHi); + indexLo = vadd_u32(indexLo, four); + indexHi = vadd_u32(indexHi, four); + } + + for( ; i+4 <= count; i+= 4 ) + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v2 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v3 = vld1q_f32_aligned_postincrement( vv ); + + float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo); + float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo); + float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo); + float32x2_t xy3 = vmul_f32( vget_low_f32(v3), vLo); + + float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x2x2_t z1 = vtrn_f32( vget_high_f32(v2), vget_high_f32(v3)); + float32x2_t zLo = vmul_f32( z0.val[0], vHi); + float32x2_t zHi = vmul_f32( z1.val[0], vHi); + + float32x2_t rLo = vpadd_f32( xy0, xy1); + float32x2_t rHi = vpadd_f32( xy2, xy3); + rLo = vadd_f32(rLo, zLo); + rHi = vadd_f32(rHi, zHi); + + uint32x2_t maskLo = vclt_f32( rLo, dotMinLo ); + uint32x2_t maskHi = vclt_f32( rHi, dotMinHi ); + dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo); + dotMinHi = vbsl_f32( maskHi, rHi, dotMinHi); + iLo = vbsl_u32(maskLo, indexLo, iLo); + iHi = vbsl_u32(maskHi, indexHi, iHi); + indexLo = vadd_u32(indexLo, four); + indexHi = vadd_u32(indexHi, four); + } + switch( count & 3 ) + { + case 3: + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v2 = vld1q_f32_aligned_postincrement( vv ); + + float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo); + float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo); + float32x2_t xy2 = vmul_f32( vget_low_f32(v2), vLo); + + float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x2_t zLo = vmul_f32( z0.val[0], vHi); + float32x2_t zHi = vmul_f32( vdup_lane_f32(vget_high_f32(v2), 0), vHi); + + float32x2_t rLo = vpadd_f32( xy0, xy1); + float32x2_t rHi = vpadd_f32( xy2, xy2); + rLo = vadd_f32(rLo, zLo); + rHi = vadd_f32(rHi, zHi); + + uint32x2_t maskLo = vclt_f32( rLo, dotMinLo ); + uint32x2_t maskHi = vclt_f32( rHi, dotMinHi ); + dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo); + dotMinHi = vbsl_f32( maskHi, rHi, dotMinHi); + iLo = vbsl_u32(maskLo, indexLo, iLo); + iHi = vbsl_u32(maskHi, indexHi, iHi); + } + break; + case 2: + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + + float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo); + float32x2_t xy1 = vmul_f32( vget_low_f32(v1), vLo); + + float32x2x2_t z0 = vtrn_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x2_t zLo = vmul_f32( z0.val[0], vHi); + + float32x2_t rLo = vpadd_f32( xy0, xy1); + rLo = vadd_f32(rLo, zLo); + + uint32x2_t maskLo = vclt_f32( rLo, dotMinLo ); + dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo); + iLo = vbsl_u32(maskLo, indexLo, iLo); + } + break; + case 1: + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x2_t xy0 = vmul_f32( vget_low_f32(v0), vLo); + float32x2_t z0 = vdup_lane_f32(vget_high_f32(v0), 0); + float32x2_t zLo = vmul_f32( z0, vHi); + float32x2_t rLo = vpadd_f32( xy0, xy0); + rLo = vadd_f32(rLo, zLo); + uint32x2_t maskLo = vclt_f32( rLo, dotMinLo ); + dotMinLo = vbsl_f32( maskLo, rLo, dotMinLo); + iLo = vbsl_u32(maskLo, indexLo, iLo); + } + break; + + default: + break; + } + + // select best answer between hi and lo results + uint32x2_t mask = vclt_f32( dotMinHi, dotMinLo ); + dotMinLo = vbsl_f32(mask, dotMinHi, dotMinLo); + iLo = vbsl_u32(mask, iHi, iLo); + + // select best answer between even and odd results + dotMinHi = vdup_lane_f32(dotMinLo, 1); + iHi = vdup_lane_u32(iLo, 1); + mask = vclt_f32( dotMinHi, dotMinLo ); + dotMinLo = vbsl_f32(mask, dotMinHi, dotMinLo); + iLo = vbsl_u32(mask, iHi, iLo); + + *dotResult = vget_lane_f32( dotMinLo, 0); + return vget_lane_u32(iLo, 0); +} + +long b3_mindot_large_v1( const float *vv, const float *vec, unsigned long count, float *dotResult ) +{ + float32x4_t vvec = vld1q_f32_aligned_postincrement( vec ); + float32x4_t vLo = vcombine_f32(vget_low_f32(vvec), vget_low_f32(vvec)); + float32x4_t vHi = vdupq_lane_f32(vget_high_f32(vvec), 0); + const uint32x4_t four = (uint32x4_t){ 4, 4, 4, 4 }; + uint32x4_t local_index = (uint32x4_t) {0, 1, 2, 3}; + uint32x4_t index = (uint32x4_t) { -1, -1, -1, -1 }; + float32x4_t minDot = (float32x4_t) { B3_INFINITY, B3_INFINITY, B3_INFINITY, B3_INFINITY }; + + unsigned long i = 0; + for( ; i + 8 <= count; i += 8 ) + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v2 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v3 = vld1q_f32_aligned_postincrement( vv ); + + // the next two lines should resolve to a single vswp d, d + float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1)); + float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3)); + // the next two lines should resolve to a single vswp d, d + float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3)); + + xy0 = vmulq_f32(xy0, vLo); + xy1 = vmulq_f32(xy1, vLo); + + float32x4x2_t zb = vuzpq_f32( z0, z1); + float32x4_t z = vmulq_f32( zb.val[0], vHi); + float32x4x2_t xy = vuzpq_f32( xy0, xy1); + float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]); + x = vaddq_f32(x, z); + + uint32x4_t mask = vcltq_f32(x, minDot); + minDot = vbslq_f32( mask, x, minDot); + index = vbslq_u32(mask, local_index, index); + local_index = vaddq_u32(local_index, four); + + v0 = vld1q_f32_aligned_postincrement( vv ); + v1 = vld1q_f32_aligned_postincrement( vv ); + v2 = vld1q_f32_aligned_postincrement( vv ); + v3 = vld1q_f32_aligned_postincrement( vv ); + + // the next two lines should resolve to a single vswp d, d + xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1)); + xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3)); + // the next two lines should resolve to a single vswp d, d + z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1)); + z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3)); + + xy0 = vmulq_f32(xy0, vLo); + xy1 = vmulq_f32(xy1, vLo); + + zb = vuzpq_f32( z0, z1); + z = vmulq_f32( zb.val[0], vHi); + xy = vuzpq_f32( xy0, xy1); + x = vaddq_f32(xy.val[0], xy.val[1]); + x = vaddq_f32(x, z); + + mask = vcltq_f32(x, minDot); + minDot = vbslq_f32( mask, x, minDot); + index = vbslq_u32(mask, local_index, index); + local_index = vaddq_u32(local_index, four); + } + + for( ; i + 4 <= count; i += 4 ) + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v2 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v3 = vld1q_f32_aligned_postincrement( vv ); + + // the next two lines should resolve to a single vswp d, d + float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1)); + float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v3)); + // the next two lines should resolve to a single vswp d, d + float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v3)); + + xy0 = vmulq_f32(xy0, vLo); + xy1 = vmulq_f32(xy1, vLo); + + float32x4x2_t zb = vuzpq_f32( z0, z1); + float32x4_t z = vmulq_f32( zb.val[0], vHi); + float32x4x2_t xy = vuzpq_f32( xy0, xy1); + float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]); + x = vaddq_f32(x, z); + + uint32x4_t mask = vcltq_f32(x, minDot); + minDot = vbslq_f32( mask, x, minDot); + index = vbslq_u32(mask, local_index, index); + local_index = vaddq_u32(local_index, four); + } + + switch (count & 3) { + case 3: + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v2 = vld1q_f32_aligned_postincrement( vv ); + + // the next two lines should resolve to a single vswp d, d + float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1)); + float32x4_t xy1 = vcombine_f32( vget_low_f32(v2), vget_low_f32(v2)); + // the next two lines should resolve to a single vswp d, d + float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1)); + float32x4_t z1 = vcombine_f32( vget_high_f32(v2), vget_high_f32(v2)); + + xy0 = vmulq_f32(xy0, vLo); + xy1 = vmulq_f32(xy1, vLo); + + float32x4x2_t zb = vuzpq_f32( z0, z1); + float32x4_t z = vmulq_f32( zb.val[0], vHi); + float32x4x2_t xy = vuzpq_f32( xy0, xy1); + float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]); + x = vaddq_f32(x, z); + + uint32x4_t mask = vcltq_f32(x, minDot); + minDot = vbslq_f32( mask, x, minDot); + index = vbslq_u32(mask, local_index, index); + local_index = vaddq_u32(local_index, four); + } + break; + + case 2: + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + float32x4_t v1 = vld1q_f32_aligned_postincrement( vv ); + + // the next two lines should resolve to a single vswp d, d + float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v1)); + // the next two lines should resolve to a single vswp d, d + float32x4_t z0 = vcombine_f32( vget_high_f32(v0), vget_high_f32(v1)); + + xy0 = vmulq_f32(xy0, vLo); + + float32x4x2_t zb = vuzpq_f32( z0, z0); + float32x4_t z = vmulq_f32( zb.val[0], vHi); + float32x4x2_t xy = vuzpq_f32( xy0, xy0); + float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]); + x = vaddq_f32(x, z); + + uint32x4_t mask = vcltq_f32(x, minDot); + minDot = vbslq_f32( mask, x, minDot); + index = vbslq_u32(mask, local_index, index); + local_index = vaddq_u32(local_index, four); + } + break; + + case 1: + { + float32x4_t v0 = vld1q_f32_aligned_postincrement( vv ); + + // the next two lines should resolve to a single vswp d, d + float32x4_t xy0 = vcombine_f32( vget_low_f32(v0), vget_low_f32(v0)); + // the next two lines should resolve to a single vswp d, d + float32x4_t z = vdupq_lane_f32(vget_high_f32(v0), 0); + + xy0 = vmulq_f32(xy0, vLo); + + z = vmulq_f32( z, vHi); + float32x4x2_t xy = vuzpq_f32( xy0, xy0); + float32x4_t x = vaddq_f32(xy.val[0], xy.val[1]); + x = vaddq_f32(x, z); + + uint32x4_t mask = vcltq_f32(x, minDot); + minDot = vbslq_f32( mask, x, minDot); + index = vbslq_u32(mask, local_index, index); + local_index = vaddq_u32(local_index, four); + } + break; + + default: + break; + } + + + // select best answer between hi and lo results + uint32x2_t mask = vclt_f32( vget_high_f32(minDot), vget_low_f32(minDot)); + float32x2_t minDot2 = vbsl_f32(mask, vget_high_f32(minDot), vget_low_f32(minDot)); + uint32x2_t index2 = vbsl_u32(mask, vget_high_u32(index), vget_low_u32(index)); + + // select best answer between even and odd results + float32x2_t minDotO = vdup_lane_f32(minDot2, 1); + uint32x2_t indexHi = vdup_lane_u32(index2, 1); + mask = vclt_f32( minDotO, minDot2 ); + minDot2 = vbsl_f32(mask, minDotO, minDot2); + index2 = vbsl_u32(mask, indexHi, index2); + + *dotResult = vget_lane_f32( minDot2, 0); + return vget_lane_u32(index2, 0); + +} + +#else + #error Unhandled __APPLE__ arch +#endif + +#endif /* __APPLE__ */ + + diff --git a/extern/bullet/src/Bullet3Common/b3Vector3.h b/extern/bullet/src/Bullet3Common/b3Vector3.h new file mode 100644 index 000000000000..16ec02b0eda6 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/b3Vector3.h @@ -0,0 +1,1344 @@ +/* +Copyright (c) 2003-2013 Gino van den Bergen / 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. +*/ + + + +#ifndef B3_VECTOR3_H +#define B3_VECTOR3_H + +//#include +#include "b3Scalar.h" +#include "b3MinMax.h" +#include "b3AlignedAllocator.h" + +#ifdef B3_USE_DOUBLE_PRECISION +#define b3Vector3Data b3Vector3DoubleData +#define b3Vector3DataName "b3Vector3DoubleData" +#else +#define b3Vector3Data b3Vector3FloatData +#define b3Vector3DataName "b3Vector3FloatData" +#endif //B3_USE_DOUBLE_PRECISION + +#if defined B3_USE_SSE + +//typedef uint32_t __m128i __attribute__ ((vector_size(16))); + +#ifdef _MSC_VER +#pragma warning(disable: 4556) // value of intrinsic immediate argument '4294967239' is out of range '0 - 255' +#endif + + +#define B3_SHUFFLE(x,y,z,w) ((w)<<6 | (z)<<4 | (y)<<2 | (x)) +//#define b3_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) ) +#define b3_pshufd_ps( _a, _mask ) _mm_shuffle_ps((_a), (_a), (_mask) ) +#define b3_splat3_ps( _a, _i ) b3_pshufd_ps((_a), B3_SHUFFLE(_i,_i,_i, 3) ) +#define b3_splat_ps( _a, _i ) b3_pshufd_ps((_a), B3_SHUFFLE(_i,_i,_i,_i) ) + +#define b3v3AbsiMask (_mm_set_epi32(0x00000000, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF)) +#define b3vAbsMask (_mm_set_epi32( 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF)) +#define b3vFFF0Mask (_mm_set_epi32(0x00000000, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF)) +#define b3v3AbsfMask b3CastiTo128f(b3v3AbsiMask) +#define b3vFFF0fMask b3CastiTo128f(b3vFFF0Mask) +#define b3vxyzMaskf b3vFFF0fMask +#define b3vAbsfMask b3CastiTo128f(b3vAbsMask) + + + +const __m128 B3_ATTRIBUTE_ALIGNED16(b3vMzeroMask) = {-0.0f, -0.0f, -0.0f, -0.0f}; +const __m128 B3_ATTRIBUTE_ALIGNED16(b3v1110) = {1.0f, 1.0f, 1.0f, 0.0f}; +const __m128 B3_ATTRIBUTE_ALIGNED16(b3vHalf) = {0.5f, 0.5f, 0.5f, 0.5f}; +const __m128 B3_ATTRIBUTE_ALIGNED16(b3v1_5) = {1.5f, 1.5f, 1.5f, 1.5f}; + +#endif + +#ifdef B3_USE_NEON + +const float32x4_t B3_ATTRIBUTE_ALIGNED16(b3vMzeroMask) = (float32x4_t){-0.0f, -0.0f, -0.0f, -0.0f}; +const int32x4_t B3_ATTRIBUTE_ALIGNED16(b3vFFF0Mask) = (int32x4_t){0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x0}; +const int32x4_t B3_ATTRIBUTE_ALIGNED16(b3vAbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF}; +const int32x4_t B3_ATTRIBUTE_ALIGNED16(b3v3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x0}; + +#endif + +class b3Vector3; +class b3Vector4; + +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) +//#if defined (B3_USE_SSE) || defined (B3_USE_NEON) +inline b3Vector3 b3MakeVector3( b3SimdFloat4 v); +inline b3Vector4 b3MakeVector4( b3SimdFloat4 vec); +#endif + +inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z); +inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z, b3Scalar w); +inline b3Vector4 b3MakeVector4(b3Scalar x,b3Scalar y,b3Scalar z,b3Scalar w); + + +/**@brief b3Vector3 can be used to represent 3D points and vectors. + * It has an un-used w component to suit 16-byte alignment when b3Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user + * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers + */ +B3_ATTRIBUTE_ALIGNED16(class) b3Vector3 +{ +public: +#if defined (B3_USE_SSE) || defined(B3_USE_NEON) // _WIN32 || ARM + union { + b3SimdFloat4 mVec128; + float m_floats[4]; + struct {float x,y,z,w;}; + + }; +#else + union + { + float m_floats[4]; + struct {float x,y,z,w;}; + }; +#endif + + +public: + + B3_DECLARE_ALIGNED_ALLOCATOR(); + +#if defined (B3_USE_SSE) || defined(B3_USE_NEON) // _WIN32 || ARM + + /*B3_FORCE_INLINE b3Vector3() + { + } + */ + + B3_FORCE_INLINE b3SimdFloat4 get128() const + { + return mVec128; + } + B3_FORCE_INLINE void set128(b3SimdFloat4 v128) + { + mVec128 = v128; + } +#endif + + public: + + + +/**@brief Add a vector to this one + * @param The vector to add to this one */ + B3_FORCE_INLINE b3Vector3& operator+=(const b3Vector3& v) + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + mVec128 = _mm_add_ps(mVec128, v.mVec128); +#elif defined(B3_USE_NEON) + mVec128 = vaddq_f32(mVec128, v.mVec128); +#else + m_floats[0] += v.m_floats[0]; + m_floats[1] += v.m_floats[1]; + m_floats[2] += v.m_floats[2]; +#endif + return *this; + } + + + /**@brief Subtract a vector from this one + * @param The vector to subtract */ + B3_FORCE_INLINE b3Vector3& operator-=(const b3Vector3& v) + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + mVec128 = _mm_sub_ps(mVec128, v.mVec128); +#elif defined(B3_USE_NEON) + mVec128 = vsubq_f32(mVec128, v.mVec128); +#else + m_floats[0] -= v.m_floats[0]; + m_floats[1] -= v.m_floats[1]; + m_floats[2] -= v.m_floats[2]; +#endif + return *this; + } + + /**@brief Scale the vector + * @param s Scale factor */ + B3_FORCE_INLINE b3Vector3& operator*=(const b3Scalar& s) + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 vs = _mm_load_ss(&s); // (S 0 0 0) + vs = b3_pshufd_ps(vs, 0x80); // (S S S 0.0) + mVec128 = _mm_mul_ps(mVec128, vs); +#elif defined(B3_USE_NEON) + mVec128 = vmulq_n_f32(mVec128, s); +#else + m_floats[0] *= s; + m_floats[1] *= s; + m_floats[2] *= s; +#endif + return *this; + } + + /**@brief Inversely scale the vector + * @param s Scale factor to divide by */ + B3_FORCE_INLINE b3Vector3& operator/=(const b3Scalar& s) + { + b3FullAssert(s != b3Scalar(0.0)); + +#if 0 //defined(B3_USE_SSE_IN_API) +// this code is not faster ! + __m128 vs = _mm_load_ss(&s); + vs = _mm_div_ss(b3v1110, vs); + vs = b3_pshufd_ps(vs, 0x00); // (S S S S) + + mVec128 = _mm_mul_ps(mVec128, vs); + + return *this; +#else + return *this *= b3Scalar(1.0) / s; +#endif + } + + /**@brief Return the dot product + * @param v The other vector in the dot product */ + B3_FORCE_INLINE b3Scalar dot(const b3Vector3& v) const + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 vd = _mm_mul_ps(mVec128, v.mVec128); + __m128 z = _mm_movehl_ps(vd, vd); + __m128 y = _mm_shuffle_ps(vd, vd, 0x55); + vd = _mm_add_ss(vd, y); + vd = _mm_add_ss(vd, z); + return _mm_cvtss_f32(vd); +#elif defined(B3_USE_NEON) + float32x4_t vd = vmulq_f32(mVec128, v.mVec128); + float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_low_f32(vd)); + x = vadd_f32(x, vget_high_f32(vd)); + return vget_lane_f32(x, 0); +#else + return m_floats[0] * v.m_floats[0] + + m_floats[1] * v.m_floats[1] + + m_floats[2] * v.m_floats[2]; +#endif + } + + /**@brief Return the length of the vector squared */ + B3_FORCE_INLINE b3Scalar length2() const + { + return dot(*this); + } + + /**@brief Return the length of the vector */ + B3_FORCE_INLINE b3Scalar length() const + { + return b3Sqrt(length2()); + } + + /**@brief Return the distance squared between the ends of this and another vector + * This is symantically treating the vector like a point */ + B3_FORCE_INLINE b3Scalar distance2(const b3Vector3& v) const; + + /**@brief Return the distance between the ends of this and another vector + * This is symantically treating the vector like a point */ + B3_FORCE_INLINE b3Scalar distance(const b3Vector3& v) const; + + B3_FORCE_INLINE b3Vector3& safeNormalize() + { + b3Scalar l2 = length2(); + //triNormal.normalize(); + if (l2 >= B3_EPSILON*B3_EPSILON) + { + (*this) /= b3Sqrt(l2); + } + else + { + setValue(1, 0, 0); + } + return *this; + } + + /**@brief Normalize this vector + * x^2 + y^2 + z^2 = 1 */ + B3_FORCE_INLINE b3Vector3& normalize() + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + // dot product first + __m128 vd = _mm_mul_ps(mVec128, mVec128); + __m128 z = _mm_movehl_ps(vd, vd); + __m128 y = _mm_shuffle_ps(vd, vd, 0x55); + vd = _mm_add_ss(vd, y); + vd = _mm_add_ss(vd, z); + + #if 0 + vd = _mm_sqrt_ss(vd); + vd = _mm_div_ss(b3v1110, vd); + vd = b3_splat_ps(vd, 0x80); + mVec128 = _mm_mul_ps(mVec128, vd); + #else + + // NR step 1/sqrt(x) - vd is x, y is output + y = _mm_rsqrt_ss(vd); // estimate + + // one step NR + z = b3v1_5; + vd = _mm_mul_ss(vd, b3vHalf); // vd * 0.5 + //x2 = vd; + vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0 + vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0 * y0 + z = _mm_sub_ss(z, vd); // 1.5 - vd * 0.5 * y0 * y0 + + y = _mm_mul_ss(y, z); // y0 * (1.5 - vd * 0.5 * y0 * y0) + + y = b3_splat_ps(y, 0x80); + mVec128 = _mm_mul_ps(mVec128, y); + + #endif + + + return *this; +#else + return *this /= length(); +#endif + } + + /**@brief Return a normalized version of this vector */ + B3_FORCE_INLINE b3Vector3 normalized() const; + + /**@brief Return a rotated version of this vector + * @param wAxis The axis to rotate about + * @param angle The angle to rotate by */ + B3_FORCE_INLINE b3Vector3 rotate( const b3Vector3& wAxis, const b3Scalar angle ) const; + + /**@brief Return the angle between this and another vector + * @param v The other vector */ + B3_FORCE_INLINE b3Scalar angle(const b3Vector3& v) const + { + b3Scalar s = b3Sqrt(length2() * v.length2()); + b3FullAssert(s != b3Scalar(0.0)); + return b3Acos(dot(v) / s); + } + + /**@brief Return a vector will the absolute values of each element */ + B3_FORCE_INLINE b3Vector3 absolute() const + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + return b3MakeVector3(_mm_and_ps(mVec128, b3v3AbsfMask)); +#elif defined(B3_USE_NEON) + return b3Vector3(vabsq_f32(mVec128)); +#else + return b3MakeVector3( + b3Fabs(m_floats[0]), + b3Fabs(m_floats[1]), + b3Fabs(m_floats[2])); +#endif + } + + /**@brief Return the cross product between this and another vector + * @param v The other vector */ + B3_FORCE_INLINE b3Vector3 cross(const b3Vector3& v) const + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 T, V; + + T = b3_pshufd_ps(mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) + V = b3_pshufd_ps(v.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) + + V = _mm_mul_ps(V, mVec128); + T = _mm_mul_ps(T, v.mVec128); + V = _mm_sub_ps(V, T); + + V = b3_pshufd_ps(V, B3_SHUFFLE(1, 2, 0, 3)); + return b3MakeVector3(V); +#elif defined(B3_USE_NEON) + float32x4_t T, V; + // form (Y, Z, X, _) of mVec128 and v.mVec128 + float32x2_t Tlow = vget_low_f32(mVec128); + float32x2_t Vlow = vget_low_f32(v.mVec128); + T = vcombine_f32(vext_f32(Tlow, vget_high_f32(mVec128), 1), Tlow); + V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v.mVec128), 1), Vlow); + + V = vmulq_f32(V, mVec128); + T = vmulq_f32(T, v.mVec128); + V = vsubq_f32(V, T); + Vlow = vget_low_f32(V); + // form (Y, Z, X, _); + V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow); + V = (float32x4_t)vandq_s32((int32x4_t)V, b3vFFF0Mask); + + return b3Vector3(V); +#else + return b3MakeVector3( + m_floats[1] * v.m_floats[2] - m_floats[2] * v.m_floats[1], + m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2], + m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]); +#endif + } + + B3_FORCE_INLINE b3Scalar triple(const b3Vector3& v1, const b3Vector3& v2) const + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + // cross: + __m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) + __m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0) + + V = _mm_mul_ps(V, v1.mVec128); + T = _mm_mul_ps(T, v2.mVec128); + V = _mm_sub_ps(V, T); + + V = _mm_shuffle_ps(V, V, B3_SHUFFLE(1, 2, 0, 3)); + + // dot: + V = _mm_mul_ps(V, mVec128); + __m128 z = _mm_movehl_ps(V, V); + __m128 y = _mm_shuffle_ps(V, V, 0x55); + V = _mm_add_ss(V, y); + V = _mm_add_ss(V, z); + return _mm_cvtss_f32(V); + +#elif defined(B3_USE_NEON) + // cross: + float32x4_t T, V; + // form (Y, Z, X, _) of mVec128 and v.mVec128 + float32x2_t Tlow = vget_low_f32(v1.mVec128); + float32x2_t Vlow = vget_low_f32(v2.mVec128); + T = vcombine_f32(vext_f32(Tlow, vget_high_f32(v1.mVec128), 1), Tlow); + V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v2.mVec128), 1), Vlow); + + V = vmulq_f32(V, v1.mVec128); + T = vmulq_f32(T, v2.mVec128); + V = vsubq_f32(V, T); + Vlow = vget_low_f32(V); + // form (Y, Z, X, _); + V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow); + + // dot: + V = vmulq_f32(mVec128, V); + float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(V)); + x = vadd_f32(x, vget_high_f32(V)); + return vget_lane_f32(x, 0); +#else + return + m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + + m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + + m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]); +#endif + } + + /**@brief Return the axis with the smallest value + * Note return values are 0,1,2 for x, y, or z */ + B3_FORCE_INLINE int minAxis() const + { + return m_floats[0] < m_floats[1] ? (m_floats[0] return this, t=1 => return other) */ + B3_FORCE_INLINE b3Vector3 lerp(const b3Vector3& v, const b3Scalar& t) const + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 vt = _mm_load_ss(&t); // (t 0 0 0) + vt = b3_pshufd_ps(vt, 0x80); // (rt rt rt 0.0) + __m128 vl = _mm_sub_ps(v.mVec128, mVec128); + vl = _mm_mul_ps(vl, vt); + vl = _mm_add_ps(vl, mVec128); + + return b3MakeVector3(vl); +#elif defined(B3_USE_NEON) + float32x4_t vl = vsubq_f32(v.mVec128, mVec128); + vl = vmulq_n_f32(vl, t); + vl = vaddq_f32(vl, mVec128); + + return b3Vector3(vl); +#else + return + b3MakeVector3( m_floats[0] + (v.m_floats[0] - m_floats[0]) * t, + m_floats[1] + (v.m_floats[1] - m_floats[1]) * t, + m_floats[2] + (v.m_floats[2] - m_floats[2]) * t); +#endif + } + + /**@brief Elementwise multiply this vector by the other + * @param v The other vector */ + B3_FORCE_INLINE b3Vector3& operator*=(const b3Vector3& v) + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + mVec128 = _mm_mul_ps(mVec128, v.mVec128); +#elif defined(B3_USE_NEON) + mVec128 = vmulq_f32(mVec128, v.mVec128); +#else + m_floats[0] *= v.m_floats[0]; + m_floats[1] *= v.m_floats[1]; + m_floats[2] *= v.m_floats[2]; +#endif + return *this; + } + + /**@brief Return the x value */ + B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; } + /**@brief Return the y value */ + B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; } + /**@brief Return the z value */ + B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; } +/**@brief Return the w value */ + B3_FORCE_INLINE const b3Scalar& getW() const { return m_floats[3]; } + + /**@brief Set the x value */ + B3_FORCE_INLINE void setX(b3Scalar _x) { m_floats[0] = _x;}; + /**@brief Set the y value */ + B3_FORCE_INLINE void setY(b3Scalar _y) { m_floats[1] = _y;}; + /**@brief Set the z value */ + B3_FORCE_INLINE void setZ(b3Scalar _z) { m_floats[2] = _z;}; + /**@brief Set the w value */ + B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w;}; + + //B3_FORCE_INLINE b3Scalar& operator[](int i) { return (&m_floats[0])[i]; } + //B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; } + ///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons. + B3_FORCE_INLINE operator b3Scalar *() { return &m_floats[0]; } + B3_FORCE_INLINE operator const b3Scalar *() const { return &m_floats[0]; } + + B3_FORCE_INLINE bool operator==(const b3Vector3& other) const + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128))); +#else + return ((m_floats[3]==other.m_floats[3]) && + (m_floats[2]==other.m_floats[2]) && + (m_floats[1]==other.m_floats[1]) && + (m_floats[0]==other.m_floats[0])); +#endif + } + + B3_FORCE_INLINE bool operator!=(const b3Vector3& other) const + { + return !(*this == other); + } + + /**@brief Set each element to the max of the current values and the values of another b3Vector3 + * @param other The other b3Vector3 to compare with + */ + B3_FORCE_INLINE void setMax(const b3Vector3& other) + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + mVec128 = _mm_max_ps(mVec128, other.mVec128); +#elif defined(B3_USE_NEON) + mVec128 = vmaxq_f32(mVec128, other.mVec128); +#else + b3SetMax(m_floats[0], other.m_floats[0]); + b3SetMax(m_floats[1], other.m_floats[1]); + b3SetMax(m_floats[2], other.m_floats[2]); + b3SetMax(m_floats[3], other.m_floats[3]); +#endif + } + + /**@brief Set each element to the min of the current values and the values of another b3Vector3 + * @param other The other b3Vector3 to compare with + */ + B3_FORCE_INLINE void setMin(const b3Vector3& other) + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + mVec128 = _mm_min_ps(mVec128, other.mVec128); +#elif defined(B3_USE_NEON) + mVec128 = vminq_f32(mVec128, other.mVec128); +#else + b3SetMin(m_floats[0], other.m_floats[0]); + b3SetMin(m_floats[1], other.m_floats[1]); + b3SetMin(m_floats[2], other.m_floats[2]); + b3SetMin(m_floats[3], other.m_floats[3]); +#endif + } + + B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z) + { + m_floats[0]=_x; + m_floats[1]=_y; + m_floats[2]=_z; + m_floats[3] = b3Scalar(0.f); + } + + void getSkewSymmetricMatrix(b3Vector3* v0,b3Vector3* v1,b3Vector3* v2) const + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + + __m128 V = _mm_and_ps(mVec128, b3vFFF0fMask); + __m128 V0 = _mm_xor_ps(b3vMzeroMask, V); + __m128 V2 = _mm_movelh_ps(V0, V); + + __m128 V1 = _mm_shuffle_ps(V, V0, 0xCE); + + V0 = _mm_shuffle_ps(V0, V, 0xDB); + V2 = _mm_shuffle_ps(V2, V, 0xF9); + + v0->mVec128 = V0; + v1->mVec128 = V1; + v2->mVec128 = V2; +#else + v0->setValue(0. ,-getZ() ,getY()); + v1->setValue(getZ() ,0. ,-getX()); + v2->setValue(-getY() ,getX() ,0.); +#endif + } + + void setZero() + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + mVec128 = (__m128)_mm_xor_ps(mVec128, mVec128); +#elif defined(B3_USE_NEON) + int32x4_t vi = vdupq_n_s32(0); + mVec128 = vreinterpretq_f32_s32(vi); +#else + setValue(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); +#endif + } + + B3_FORCE_INLINE bool isZero() const + { + return m_floats[0] == b3Scalar(0) && m_floats[1] == b3Scalar(0) && m_floats[2] == b3Scalar(0); + } + + B3_FORCE_INLINE bool fuzzyZero() const + { + return length2() < B3_EPSILON; + } + + B3_FORCE_INLINE void serialize(struct b3Vector3Data& dataOut) const; + + B3_FORCE_INLINE void deSerialize(const struct b3Vector3Data& dataIn); + + B3_FORCE_INLINE void serializeFloat(struct b3Vector3FloatData& dataOut) const; + + B3_FORCE_INLINE void deSerializeFloat(const struct b3Vector3FloatData& dataIn); + + B3_FORCE_INLINE void serializeDouble(struct b3Vector3DoubleData& dataOut) const; + + B3_FORCE_INLINE void deSerializeDouble(const struct b3Vector3DoubleData& dataIn); + + /**@brief returns index of maximum dot product between this and vectors in array[] + * @param array The other vectors + * @param array_count The number of other vectors + * @param dotOut The maximum dot product */ + B3_FORCE_INLINE long maxDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const; + + /**@brief returns index of minimum dot product between this and vectors in array[] + * @param array The other vectors + * @param array_count The number of other vectors + * @param dotOut The minimum dot product */ + B3_FORCE_INLINE long minDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const; + + /* create a vector as b3Vector3( this->dot( b3Vector3 v0 ), this->dot( b3Vector3 v1), this->dot( b3Vector3 v2 )) */ + B3_FORCE_INLINE b3Vector3 dot3( const b3Vector3 &v0, const b3Vector3 &v1, const b3Vector3 &v2 ) const + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + + __m128 a0 = _mm_mul_ps( v0.mVec128, this->mVec128 ); + __m128 a1 = _mm_mul_ps( v1.mVec128, this->mVec128 ); + __m128 a2 = _mm_mul_ps( v2.mVec128, this->mVec128 ); + __m128 b0 = _mm_unpacklo_ps( a0, a1 ); + __m128 b1 = _mm_unpackhi_ps( a0, a1 ); + __m128 b2 = _mm_unpacklo_ps( a2, _mm_setzero_ps() ); + __m128 r = _mm_movelh_ps( b0, b2 ); + r = _mm_add_ps( r, _mm_movehl_ps( b2, b0 )); + a2 = _mm_and_ps( a2, b3vxyzMaskf); + r = _mm_add_ps( r, b3CastdTo128f (_mm_move_sd( b3CastfTo128d(a2), b3CastfTo128d(b1) ))); + return b3MakeVector3(r); + +#elif defined(B3_USE_NEON) + static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 }; + float32x4_t a0 = vmulq_f32( v0.mVec128, this->mVec128); + float32x4_t a1 = vmulq_f32( v1.mVec128, this->mVec128); + float32x4_t a2 = vmulq_f32( v2.mVec128, this->mVec128); + float32x2x2_t zLo = vtrn_f32( vget_high_f32(a0), vget_high_f32(a1)); + a2 = (float32x4_t) vandq_u32((uint32x4_t) a2, xyzMask ); + float32x2_t b0 = vadd_f32( vpadd_f32( vget_low_f32(a0), vget_low_f32(a1)), zLo.val[0] ); + float32x2_t b1 = vpadd_f32( vpadd_f32( vget_low_f32(a2), vget_high_f32(a2)), vdup_n_f32(0.0f)); + return b3Vector3( vcombine_f32(b0, b1) ); +#else + return b3MakeVector3( dot(v0), dot(v1), dot(v2)); +#endif + } +}; + +/**@brief Return the sum of two vectors (Point symantics)*/ +B3_FORCE_INLINE b3Vector3 +operator+(const b3Vector3& v1, const b3Vector3& v2) +{ +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + return b3MakeVector3(_mm_add_ps(v1.mVec128, v2.mVec128)); +#elif defined(B3_USE_NEON) + return b3MakeVector3(vaddq_f32(v1.mVec128, v2.mVec128)); +#else + return b3MakeVector3( + v1.m_floats[0] + v2.m_floats[0], + v1.m_floats[1] + v2.m_floats[1], + v1.m_floats[2] + v2.m_floats[2]); +#endif +} + +/**@brief Return the elementwise product of two vectors */ +B3_FORCE_INLINE b3Vector3 +operator*(const b3Vector3& v1, const b3Vector3& v2) +{ +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + return b3MakeVector3(_mm_mul_ps(v1.mVec128, v2.mVec128)); +#elif defined(B3_USE_NEON) + return b3MakeVector3(vmulq_f32(v1.mVec128, v2.mVec128)); +#else + return b3MakeVector3( + v1.m_floats[0] * v2.m_floats[0], + v1.m_floats[1] * v2.m_floats[1], + v1.m_floats[2] * v2.m_floats[2]); +#endif +} + +/**@brief Return the difference between two vectors */ +B3_FORCE_INLINE b3Vector3 +operator-(const b3Vector3& v1, const b3Vector3& v2) +{ +#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) + + // without _mm_and_ps this code causes slowdown in Concave moving + __m128 r = _mm_sub_ps(v1.mVec128, v2.mVec128); + return b3MakeVector3(_mm_and_ps(r, b3vFFF0fMask)); +#elif defined(B3_USE_NEON) + float32x4_t r = vsubq_f32(v1.mVec128, v2.mVec128); + return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)r, b3vFFF0Mask)); +#else + return b3MakeVector3( + v1.m_floats[0] - v2.m_floats[0], + v1.m_floats[1] - v2.m_floats[1], + v1.m_floats[2] - v2.m_floats[2]); +#endif +} + +/**@brief Return the negative of the vector */ +B3_FORCE_INLINE b3Vector3 +operator-(const b3Vector3& v) +{ +#if (defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)) + __m128 r = _mm_xor_ps(v.mVec128, b3vMzeroMask); + return b3MakeVector3(_mm_and_ps(r, b3vFFF0fMask)); +#elif defined(B3_USE_NEON) + return b3MakeVector3((b3SimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)b3vMzeroMask)); +#else + return b3MakeVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]); +#endif +} + +/**@brief Return the vector scaled by s */ +B3_FORCE_INLINE b3Vector3 +operator*(const b3Vector3& v, const b3Scalar& s) +{ +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + __m128 vs = _mm_load_ss(&s); // (S 0 0 0) + vs = b3_pshufd_ps(vs, 0x80); // (S S S 0.0) + return b3MakeVector3(_mm_mul_ps(v.mVec128, vs)); +#elif defined(B3_USE_NEON) + float32x4_t r = vmulq_n_f32(v.mVec128, s); + return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)r, b3vFFF0Mask)); +#else + return b3MakeVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s); +#endif +} + +/**@brief Return the vector scaled by s */ +B3_FORCE_INLINE b3Vector3 +operator*(const b3Scalar& s, const b3Vector3& v) +{ + return v * s; +} + +/**@brief Return the vector inversely scaled by s */ +B3_FORCE_INLINE b3Vector3 +operator/(const b3Vector3& v, const b3Scalar& s) +{ + b3FullAssert(s != b3Scalar(0.0)); +#if 0 //defined(B3_USE_SSE_IN_API) +// this code is not faster ! + __m128 vs = _mm_load_ss(&s); + vs = _mm_div_ss(b3v1110, vs); + vs = b3_pshufd_ps(vs, 0x00); // (S S S S) + + return b3Vector3(_mm_mul_ps(v.mVec128, vs)); +#else + return v * (b3Scalar(1.0) / s); +#endif +} + +/**@brief Return the vector inversely scaled by s */ +B3_FORCE_INLINE b3Vector3 +operator/(const b3Vector3& v1, const b3Vector3& v2) +{ +#if (defined(B3_USE_SSE_IN_API)&& defined (B3_USE_SSE)) + __m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128); + vec = _mm_and_ps(vec, b3vFFF0fMask); + return b3MakeVector3(vec); +#elif defined(B3_USE_NEON) + float32x4_t x, y, v, m; + + x = v1.mVec128; + y = v2.mVec128; + + v = vrecpeq_f32(y); // v ~ 1/y + m = vrecpsq_f32(y, v); // m = (2-v*y) + v = vmulq_f32(v, m); // vv = v*m ~~ 1/y + m = vrecpsq_f32(y, v); // mm = (2-vv*y) + v = vmulq_f32(v, x); // x*vv + v = vmulq_f32(v, m); // (x*vv)*(2-vv*y) = x*(vv(2-vv*y)) ~~~ x/y + + return b3Vector3(v); +#else + return b3MakeVector3( + v1.m_floats[0] / v2.m_floats[0], + v1.m_floats[1] / v2.m_floats[1], + v1.m_floats[2] / v2.m_floats[2]); +#endif +} + +/**@brief Return the dot product between two vectors */ +B3_FORCE_INLINE b3Scalar +b3Dot(const b3Vector3& v1, const b3Vector3& v2) +{ + return v1.dot(v2); +} + + +/**@brief Return the distance squared between two vectors */ +B3_FORCE_INLINE b3Scalar +b3Distance2(const b3Vector3& v1, const b3Vector3& v2) +{ + return v1.distance2(v2); +} + + +/**@brief Return the distance between two vectors */ +B3_FORCE_INLINE b3Scalar +b3Distance(const b3Vector3& v1, const b3Vector3& v2) +{ + return v1.distance(v2); +} + +/**@brief Return the angle between two vectors */ +B3_FORCE_INLINE b3Scalar +b3Angle(const b3Vector3& v1, const b3Vector3& v2) +{ + return v1.angle(v2); +} + +/**@brief Return the cross product of two vectors */ +B3_FORCE_INLINE b3Vector3 +b3Cross(const b3Vector3& v1, const b3Vector3& v2) +{ + return v1.cross(v2); +} + +B3_FORCE_INLINE b3Scalar +b3Triple(const b3Vector3& v1, const b3Vector3& v2, const b3Vector3& v3) +{ + return v1.triple(v2, v3); +} + +/**@brief Return the linear interpolation between two vectors + * @param v1 One vector + * @param v2 The other vector + * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */ +B3_FORCE_INLINE b3Vector3 +b3Lerp(const b3Vector3& v1, const b3Vector3& v2, const b3Scalar& t) +{ + return v1.lerp(v2, t); +} + + + +B3_FORCE_INLINE b3Scalar b3Vector3::distance2(const b3Vector3& v) const +{ + return (v - *this).length2(); +} + +B3_FORCE_INLINE b3Scalar b3Vector3::distance(const b3Vector3& v) const +{ + return (v - *this).length(); +} + +B3_FORCE_INLINE b3Vector3 b3Vector3::normalized() const +{ +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + b3Vector3 norm = *this; + + return norm.normalize(); +#else + return *this / length(); +#endif +} + +B3_FORCE_INLINE b3Vector3 b3Vector3::rotate( const b3Vector3& wAxis, const b3Scalar _angle ) const +{ + // wAxis must be a unit lenght vector + +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + + __m128 O = _mm_mul_ps(wAxis.mVec128, mVec128); + b3Scalar ssin = b3Sin( _angle ); + __m128 C = wAxis.cross( b3MakeVector3(mVec128) ).mVec128; + O = _mm_and_ps(O, b3vFFF0fMask); + b3Scalar scos = b3Cos( _angle ); + + __m128 vsin = _mm_load_ss(&ssin); // (S 0 0 0) + __m128 vcos = _mm_load_ss(&scos); // (S 0 0 0) + + __m128 Y = b3_pshufd_ps(O, 0xC9); // (Y Z X 0) + __m128 Z = b3_pshufd_ps(O, 0xD2); // (Z X Y 0) + O = _mm_add_ps(O, Y); + vsin = b3_pshufd_ps(vsin, 0x80); // (S S S 0) + O = _mm_add_ps(O, Z); + vcos = b3_pshufd_ps(vcos, 0x80); // (S S S 0) + + vsin = vsin * C; + O = O * wAxis.mVec128; + __m128 X = mVec128 - O; + + O = O + vsin; + vcos = vcos * X; + O = O + vcos; + + return b3MakeVector3(O); +#else + b3Vector3 o = wAxis * wAxis.dot( *this ); + b3Vector3 _x = *this - o; + b3Vector3 _y; + + _y = wAxis.cross( *this ); + + return ( o + _x * b3Cos( _angle ) + _y * b3Sin( _angle ) ); +#endif +} + +B3_FORCE_INLINE long b3Vector3::maxDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const +{ +#if defined (B3_USE_SSE) || defined (B3_USE_NEON) + #if defined _WIN32 || defined (B3_USE_SSE) + const long scalar_cutoff = 10; + long b3_maxdot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut ); + #elif defined B3_USE_NEON + const long scalar_cutoff = 4; + extern long (*_maxdot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut ); + #endif + if( array_count < scalar_cutoff ) +#else + +#endif//B3_USE_SSE || B3_USE_NEON + { + b3Scalar maxDot = -B3_INFINITY; + int i = 0; + int ptIndex = -1; + for( i = 0; i < array_count; i++ ) + { + b3Scalar dot = array[i].dot(*this); + + if( dot > maxDot ) + { + maxDot = dot; + ptIndex = i; + } + } + + b3Assert(ptIndex>=0); + if (ptIndex<0) + { + ptIndex = 0; + } + dotOut = maxDot; + return ptIndex; + } +#if defined (B3_USE_SSE) || defined (B3_USE_NEON) + return b3_maxdot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut ); +#endif +} + +B3_FORCE_INLINE long b3Vector3::minDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const +{ +#if defined (B3_USE_SSE) || defined (B3_USE_NEON) + #if defined B3_USE_SSE + const long scalar_cutoff = 10; + long b3_mindot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut ); + #elif defined B3_USE_NEON + const long scalar_cutoff = 4; + extern long (*b3_mindot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut ); + #else + #error unhandled arch! + #endif + + if( array_count < scalar_cutoff ) +#endif//B3_USE_SSE || B3_USE_NEON + { + b3Scalar minDot = B3_INFINITY; + int i = 0; + int ptIndex = -1; + + for( i = 0; i < array_count; i++ ) + { + b3Scalar dot = array[i].dot(*this); + + if( dot < minDot ) + { + minDot = dot; + ptIndex = i; + } + } + + dotOut = minDot; + + return ptIndex; + } +#if defined (B3_USE_SSE) || defined (B3_USE_NEON) + return b3_mindot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut ); +#endif +} + + +class b3Vector4 : public b3Vector3 +{ +public: + + + + + + + B3_FORCE_INLINE b3Vector4 absolute4() const + { +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + return b3MakeVector4(_mm_and_ps(mVec128, b3vAbsfMask)); +#elif defined(B3_USE_NEON) + return b3Vector4(vabsq_f32(mVec128)); +#else + return b3MakeVector4( + b3Fabs(m_floats[0]), + b3Fabs(m_floats[1]), + b3Fabs(m_floats[2]), + b3Fabs(m_floats[3])); +#endif + } + + + b3Scalar getW() const { return m_floats[3];} + + + B3_FORCE_INLINE int maxAxis4() const + { + int maxIndex = -1; + b3Scalar maxVal = b3Scalar(-B3_LARGE_FLOAT); + if (m_floats[0] > maxVal) + { + maxIndex = 0; + maxVal = m_floats[0]; + } + if (m_floats[1] > maxVal) + { + maxIndex = 1; + maxVal = m_floats[1]; + } + if (m_floats[2] > maxVal) + { + maxIndex = 2; + maxVal =m_floats[2]; + } + if (m_floats[3] > maxVal) + { + maxIndex = 3; + } + + return maxIndex; + } + + + B3_FORCE_INLINE int minAxis4() const + { + int minIndex = -1; + b3Scalar minVal = b3Scalar(B3_LARGE_FLOAT); + if (m_floats[0] < minVal) + { + minIndex = 0; + minVal = m_floats[0]; + } + if (m_floats[1] < minVal) + { + minIndex = 1; + minVal = m_floats[1]; + } + if (m_floats[2] < minVal) + { + minIndex = 2; + minVal =m_floats[2]; + } + if (m_floats[3] < minVal) + { + minIndex = 3; + minVal = m_floats[3]; + } + + return minIndex; + } + + + B3_FORCE_INLINE int closestAxis4() const + { + return absolute4().maxAxis4(); + } + + + + + /**@brief Set x,y,z and zero w + * @param x Value of x + * @param y Value of y + * @param z Value of z + */ + + +/* void getValue(b3Scalar *m) const + { + m[0] = m_floats[0]; + m[1] = m_floats[1]; + m[2] =m_floats[2]; + } +*/ +/**@brief Set the values + * @param x Value of x + * @param y Value of y + * @param z Value of z + * @param w Value of w + */ + B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w) + { + m_floats[0]=_x; + m_floats[1]=_y; + m_floats[2]=_z; + m_floats[3]=_w; + } + + +}; + + +///b3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization +B3_FORCE_INLINE void b3SwapScalarEndian(const b3Scalar& sourceVal, b3Scalar& destVal) +{ + #ifdef B3_USE_DOUBLE_PRECISION + unsigned char* dest = (unsigned char*) &destVal; + unsigned char* src = (unsigned char*) &sourceVal; + dest[0] = src[7]; + dest[1] = src[6]; + dest[2] = src[5]; + dest[3] = src[4]; + dest[4] = src[3]; + dest[5] = src[2]; + dest[6] = src[1]; + dest[7] = src[0]; +#else + unsigned char* dest = (unsigned char*) &destVal; + unsigned char* src = (unsigned char*) &sourceVal; + dest[0] = src[3]; + dest[1] = src[2]; + dest[2] = src[1]; + dest[3] = src[0]; +#endif //B3_USE_DOUBLE_PRECISION +} +///b3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization +B3_FORCE_INLINE void b3SwapVector3Endian(const b3Vector3& sourceVec, b3Vector3& destVec) +{ + for (int i=0;i<4;i++) + { + b3SwapScalarEndian(sourceVec[i],destVec[i]); + } + +} + +///b3UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization +B3_FORCE_INLINE void b3UnSwapVector3Endian(b3Vector3& vector) +{ + + b3Vector3 swappedVec; + for (int i=0;i<4;i++) + { + b3SwapScalarEndian(vector[i],swappedVec[i]); + } + vector = swappedVec; +} + +template +B3_FORCE_INLINE void b3PlaneSpace1 (const T& n, T& p, T& q) +{ + if (b3Fabs(n[2]) > B3_SQRT12) { + // choose p in y-z plane + b3Scalar a = n[1]*n[1] + n[2]*n[2]; + b3Scalar k = b3RecipSqrt (a); + p[0] = 0; + p[1] = -n[2]*k; + p[2] = n[1]*k; + // set q = n x p + q[0] = a*k; + q[1] = -n[0]*p[2]; + q[2] = n[0]*p[1]; + } + else { + // choose p in x-y plane + b3Scalar a = n[0]*n[0] + n[1]*n[1]; + b3Scalar k = b3RecipSqrt (a); + p[0] = -n[1]*k; + p[1] = n[0]*k; + p[2] = 0; + // set q = n x p + q[0] = -n[2]*p[1]; + q[1] = n[2]*p[0]; + q[2] = a*k; + } +} + + +struct b3Vector3FloatData +{ + float m_floats[4]; +}; + +struct b3Vector3DoubleData +{ + double m_floats[4]; + +}; + +B3_FORCE_INLINE void b3Vector3::serializeFloat(struct b3Vector3FloatData& dataOut) const +{ + ///could also do a memcpy, check if it is worth it + for (int i=0;i<4;i++) + dataOut.m_floats[i] = float(m_floats[i]); +} + +B3_FORCE_INLINE void b3Vector3::deSerializeFloat(const struct b3Vector3FloatData& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = b3Scalar(dataIn.m_floats[i]); +} + + +B3_FORCE_INLINE void b3Vector3::serializeDouble(struct b3Vector3DoubleData& dataOut) const +{ + ///could also do a memcpy, check if it is worth it + for (int i=0;i<4;i++) + dataOut.m_floats[i] = double(m_floats[i]); +} + +B3_FORCE_INLINE void b3Vector3::deSerializeDouble(const struct b3Vector3DoubleData& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = b3Scalar(dataIn.m_floats[i]); +} + + +B3_FORCE_INLINE void b3Vector3::serialize(struct b3Vector3Data& dataOut) const +{ + ///could also do a memcpy, check if it is worth it + for (int i=0;i<4;i++) + dataOut.m_floats[i] = m_floats[i]; +} + +B3_FORCE_INLINE void b3Vector3::deSerialize(const struct b3Vector3Data& dataIn) +{ + for (int i=0;i<4;i++) + m_floats[i] = dataIn.m_floats[i]; +} + + + + +inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z) +{ + b3Vector3 tmp; + tmp.setValue(x,y,z); + return tmp; +} + +inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z, b3Scalar w) +{ + b3Vector3 tmp; + tmp.setValue(x,y,z); + tmp.w = w; + return tmp; +} + +inline b3Vector4 b3MakeVector4(b3Scalar x,b3Scalar y,b3Scalar z,b3Scalar w) +{ + b3Vector4 tmp; + tmp.setValue(x,y,z,w); + return tmp; +} + +#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE) + +inline b3Vector3 b3MakeVector3( b3SimdFloat4 v) +{ + b3Vector3 tmp; + tmp.set128(v); + return tmp; +} + +inline b3Vector4 b3MakeVector4(b3SimdFloat4 vec) +{ + b3Vector4 tmp; + tmp.set128(vec); + return tmp; +} + +#endif + + +#endif //B3_VECTOR3_H diff --git a/extern/bullet/src/Bullet3Common/premake4.lua b/extern/bullet/src/Bullet3Common/premake4.lua new file mode 100644 index 000000000000..f03573ec2d2e --- /dev/null +++ b/extern/bullet/src/Bullet3Common/premake4.lua @@ -0,0 +1,16 @@ + project "Bullet3Common" + + language "C++" + + kind "StaticLib" + + if os.is("Linux") then + buildoptions{"-fPIC"} + end + + includedirs {".."} + + files { + "*.cpp", + "*.h" + } diff --git a/extern/bullet/src/Bullet3Common/shared/b3Float4.h b/extern/bullet/src/Bullet3Common/shared/b3Float4.h new file mode 100644 index 000000000000..5e4b95bcee15 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/shared/b3Float4.h @@ -0,0 +1,97 @@ +#ifndef B3_FLOAT4_H +#define B3_FLOAT4_H + +#include "Bullet3Common/shared/b3PlatformDefinitions.h" + +#ifdef __cplusplus + #include "Bullet3Common/b3Vector3.h" + #define b3Float4 b3Vector3 + #define b3Float4ConstArg const b3Vector3& + #define b3Dot3F4 b3Dot + #define b3Cross3 b3Cross + #define b3MakeFloat4 b3MakeVector3 + inline b3Vector3 b3Normalized(const b3Vector3& vec) + { + return vec.normalized(); + } + + inline b3Float4 b3FastNormalized3(b3Float4ConstArg v) + { + return v.normalized(); + } + + inline b3Float4 b3MaxFloat4 (const b3Float4& a, const b3Float4& b) + { + b3Float4 tmp = a; + tmp.setMax(b); + return tmp; + } + inline b3Float4 b3MinFloat4 (const b3Float4& a, const b3Float4& b) + { + b3Float4 tmp = a; + tmp.setMin(b); + return tmp; + } + + + +#else + typedef float4 b3Float4; + #define b3Float4ConstArg const b3Float4 + #define b3MakeFloat4 (float4) + float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1) + { + float4 a1 = b3MakeFloat4(v0.xyz,0.f); + float4 b1 = b3MakeFloat4(v1.xyz,0.f); + return dot(a1, b1); + } + b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1) + { + float4 a1 = b3MakeFloat4(v0.xyz,0.f); + float4 b1 = b3MakeFloat4(v1.xyz,0.f); + return cross(a1, b1); + } + #define b3MinFloat4 min + #define b3MaxFloat4 max + + #define b3Normalized(a) normalize(a) + +#endif + + + +inline bool b3IsAlmostZero(b3Float4ConstArg v) +{ + if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) + return false; + return true; +} + + +inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut ) +{ + float maxDot = -B3_INFINITY; + int i = 0; + int ptIndex = -1; + for( i = 0; i < vecLen; i++ ) + { + float dot = b3Dot3F4(vecArray[i],vec); + + if( dot > maxDot ) + { + maxDot = dot; + ptIndex = i; + } + } + b3Assert(ptIndex>=0); + if (ptIndex<0) + { + ptIndex = 0; + } + *dotOut = maxDot; + return ptIndex; +} + + + +#endif //B3_FLOAT4_H diff --git a/extern/bullet/src/Bullet3Common/shared/b3Int2.h b/extern/bullet/src/Bullet3Common/shared/b3Int2.h new file mode 100644 index 000000000000..f1d01f81a570 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/shared/b3Int2.h @@ -0,0 +1,64 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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. +*/ + +#ifndef B3_INT2_H +#define B3_INT2_H + +#ifdef __cplusplus + +struct b3UnsignedInt2 +{ + union + { + struct + { + unsigned int x,y; + }; + struct + { + unsigned int s[2]; + }; + }; +}; + +struct b3Int2 +{ + union + { + struct + { + int x,y; + }; + struct + { + int s[2]; + }; + }; +}; + +inline b3Int2 b3MakeInt2(int x, int y) +{ + b3Int2 v; + v.s[0] = x; v.s[1] = y; + return v; +} +#else + +#define b3UnsignedInt2 uint2 +#define b3Int2 int2 +#define b3MakeInt2 (int2) + +#endif //__cplusplus +#endif \ No newline at end of file diff --git a/extern/bullet/src/Bullet3Common/shared/b3Int4.h b/extern/bullet/src/Bullet3Common/shared/b3Int4.h new file mode 100644 index 000000000000..aa02d6beef90 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/shared/b3Int4.h @@ -0,0 +1,68 @@ +#ifndef B3_INT4_H +#define B3_INT4_H + +#ifdef __cplusplus + +#include "Bullet3Common/b3Scalar.h" + + +B3_ATTRIBUTE_ALIGNED16(struct) b3UnsignedInt4 +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); + + union + { + struct + { + unsigned int x,y,z,w; + }; + struct + { + unsigned int s[4]; + }; + }; +}; + +B3_ATTRIBUTE_ALIGNED16(struct) b3Int4 +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); + + union + { + struct + { + int x,y,z,w; + }; + struct + { + int s[4]; + }; + }; +}; + +B3_FORCE_INLINE b3Int4 b3MakeInt4(int x, int y, int z, int w = 0) +{ + b3Int4 v; + v.s[0] = x; v.s[1] = y; v.s[2] = z; v.s[3] = w; + return v; +} + +B3_FORCE_INLINE b3UnsignedInt4 b3MakeUnsignedInt4(unsigned int x, unsigned int y, unsigned int z, unsigned int w = 0) +{ + b3UnsignedInt4 v; + v.s[0] = x; v.s[1] = y; v.s[2] = z; v.s[3] = w; + return v; +} + +#else + + +#define b3UnsignedInt4 uint4 +#define b3Int4 int4 +#define b3MakeInt4 (int4) +#define b3MakeUnsignedInt4 (uint4) + + +#endif //__cplusplus + +#endif //B3_INT4_H diff --git a/extern/bullet/src/Bullet3Common/shared/b3Mat3x3.h b/extern/bullet/src/Bullet3Common/shared/b3Mat3x3.h new file mode 100644 index 000000000000..7b1fef32f84b --- /dev/null +++ b/extern/bullet/src/Bullet3Common/shared/b3Mat3x3.h @@ -0,0 +1,179 @@ + +#ifndef B3_MAT3x3_H +#define B3_MAT3x3_H + +#include "Bullet3Common/shared/b3Quat.h" + + +#ifdef __cplusplus + +#include "Bullet3Common/b3Matrix3x3.h" + +#define b3Mat3x3 b3Matrix3x3 +#define b3Mat3x3ConstArg const b3Matrix3x3& + +inline b3Mat3x3 b3QuatGetRotationMatrix(b3QuatConstArg quat) +{ + return b3Mat3x3(quat); +} + +inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg mat) +{ + return mat.absolute(); +} + +#define b3GetRow(m,row) m.getRow(row) + +__inline +b3Float4 mtMul3(b3Float4ConstArg a, b3Mat3x3ConstArg b) +{ + return b*a; +} + + +#else + +typedef struct +{ + b3Float4 m_row[3]; +}b3Mat3x3; + +#define b3Mat3x3ConstArg const b3Mat3x3 +#define b3GetRow(m,row) (m.m_row[row]) + +inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat) +{ + b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f); + b3Mat3x3 out; + + out.m_row[0].x=1-2*quat2.y-2*quat2.z; + out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z; + out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y; + out.m_row[0].w = 0.f; + + out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z; + out.m_row[1].y=1-2*quat2.x-2*quat2.z; + out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x; + out.m_row[1].w = 0.f; + + out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y; + out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x; + out.m_row[2].z=1-2*quat2.x-2*quat2.y; + out.m_row[2].w = 0.f; + + return out; +} + +inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn) +{ + b3Mat3x3 out; + out.m_row[0] = fabs(matIn.m_row[0]); + out.m_row[1] = fabs(matIn.m_row[1]); + out.m_row[2] = fabs(matIn.m_row[2]); + return out; +} + + +__inline +b3Mat3x3 mtZero(); + +__inline +b3Mat3x3 mtIdentity(); + +__inline +b3Mat3x3 mtTranspose(b3Mat3x3 m); + +__inline +b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b); + +__inline +b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b); + +__inline +b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b); + +__inline +b3Mat3x3 mtZero() +{ + b3Mat3x3 m; + m.m_row[0] = (b3Float4)(0.f); + m.m_row[1] = (b3Float4)(0.f); + m.m_row[2] = (b3Float4)(0.f); + return m; +} + +__inline +b3Mat3x3 mtIdentity() +{ + b3Mat3x3 m; + m.m_row[0] = (b3Float4)(1,0,0,0); + m.m_row[1] = (b3Float4)(0,1,0,0); + m.m_row[2] = (b3Float4)(0,0,1,0); + return m; +} + +__inline +b3Mat3x3 mtTranspose(b3Mat3x3 m) +{ + b3Mat3x3 out; + out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f); + out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f); + out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f); + return out; +} + +__inline +b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b) +{ + b3Mat3x3 transB; + transB = mtTranspose( b ); + b3Mat3x3 ans; + // why this doesn't run when 0ing in the for{} + a.m_row[0].w = 0.f; + a.m_row[1].w = 0.f; + a.m_row[2].w = 0.f; + for(int i=0; i<3; i++) + { +// a.m_row[i].w = 0.f; + ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]); + ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]); + ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]); + ans.m_row[i].w = 0.f; + } + return ans; +} + +__inline +b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b) +{ + b3Float4 ans; + ans.x = b3Dot3F4( a.m_row[0], b ); + ans.y = b3Dot3F4( a.m_row[1], b ); + ans.z = b3Dot3F4( a.m_row[2], b ); + ans.w = 0.f; + return ans; +} + +__inline +b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b) +{ + b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0); + b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0); + b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0); + + b3Float4 ans; + ans.x = b3Dot3F4( a, colx ); + ans.y = b3Dot3F4( a, coly ); + ans.z = b3Dot3F4( a, colz ); + return ans; +} + + +#endif + + + + + + +#endif //B3_MAT3x3_H diff --git a/extern/bullet/src/Bullet3Common/shared/b3PlatformDefinitions.h b/extern/bullet/src/Bullet3Common/shared/b3PlatformDefinitions.h new file mode 100644 index 000000000000..1c133fb08896 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/shared/b3PlatformDefinitions.h @@ -0,0 +1,41 @@ +#ifndef B3_PLATFORM_DEFINITIONS_H +#define B3_PLATFORM_DEFINITIONS_H + +struct MyTest +{ + int bla; +}; + +#ifdef __cplusplus +//#define b3ConstArray(a) const b3AlignedObjectArray& +#define b3ConstArray(a) const a* +#define b3AtomicInc(a) ((*a)++) + +inline int b3AtomicAdd (volatile int *p, int val) +{ + int oldValue = *p; + int newValue = oldValue+val; + *p = newValue; + return oldValue; +} + +#define __global + +#define B3_STATIC static +#else +//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX +#define B3_LARGE_FLOAT 1e18f +#define B3_INFINITY 1e18f +#define b3Assert(a) +#define b3ConstArray(a) __global const a* +#define b3AtomicInc atomic_inc +#define b3AtomicAdd atomic_add +#define b3Fabs fabs +#define b3Sqrt native_sqrt +#define b3Sin native_sin +#define b3Cos native_cos + +#define B3_STATIC +#endif + +#endif diff --git a/extern/bullet/src/Bullet3Common/shared/b3Quat.h b/extern/bullet/src/Bullet3Common/shared/b3Quat.h new file mode 100644 index 000000000000..f262d5e08f41 --- /dev/null +++ b/extern/bullet/src/Bullet3Common/shared/b3Quat.h @@ -0,0 +1,103 @@ +#ifndef B3_QUAT_H +#define B3_QUAT_H + +#include "Bullet3Common/shared/b3PlatformDefinitions.h" +#include "Bullet3Common/shared/b3Float4.h" + +#ifdef __cplusplus + #include "Bullet3Common/b3Quaternion.h" + #include "Bullet3Common/b3Transform.h" + + #define b3Quat b3Quaternion + #define b3QuatConstArg const b3Quaternion& + inline b3Quat b3QuatInverse(b3QuatConstArg orn) + { + return orn.inverse(); + } + + inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation) + { + b3Transform tr; + tr.setOrigin(translation); + tr.setRotation(orientation); + return tr(point); + } + +#else + typedef float4 b3Quat; + #define b3QuatConstArg const b3Quat + + +inline float4 b3FastNormalize4(float4 v) +{ + v = (float4)(v.xyz,0.f); + return fast_normalize(v); +} + +inline b3Quat b3QuatMul(b3Quat a, b3Quat b); +inline b3Quat b3QuatNormalized(b3QuatConstArg in); +inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec); +inline b3Quat b3QuatInvert(b3QuatConstArg q); +inline b3Quat b3QuatInverse(b3QuatConstArg q); + +inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b) +{ + b3Quat ans; + ans = b3Cross3( a, b ); + ans += a.w*b+b.w*a; +// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - b3Dot3F4(a, b); + return ans; +} + +inline b3Quat b3QuatNormalized(b3QuatConstArg in) +{ + b3Quat q; + q=in; + //return b3FastNormalize4(in); + float len = native_sqrt(dot(q, q)); + if(len > 0.f) + { + q *= 1.f / len; + } + else + { + q.x = q.y = q.z = 0.f; + q.w = 1.f; + } + return q; +} +inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec) +{ + b3Quat qInv = b3QuatInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv); + return out; +} + + + +inline b3Quat b3QuatInverse(b3QuatConstArg q) +{ + return (b3Quat)(-q.xyz, q.w); +} + +inline b3Quat b3QuatInvert(b3QuatConstArg q) +{ + return (b3Quat)(-q.xyz, q.w); +} + +inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec) +{ + return b3QuatRotate( b3QuatInvert( q ), vec ); +} + +inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation) +{ + return b3QuatRotate( orientation, point ) + (translation); +} + +#endif + +#endif //B3_QUAT_H diff --git a/extern/bullet/src/Bullet3Dynamics/CMakeLists.txt b/extern/bullet/src/Bullet3Dynamics/CMakeLists.txt new file mode 100644 index 000000000000..94c120d9b54f --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/CMakeLists.txt @@ -0,0 +1,61 @@ + +INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/src +) + +SET(Bullet3Dynamics_SRCS + b3CpuRigidBodyPipeline.cpp + ConstraintSolver/b3FixedConstraint.cpp + ConstraintSolver/b3Generic6DofConstraint.cpp + ConstraintSolver/b3PgsJacobiSolver.cpp + ConstraintSolver/b3Point2PointConstraint.cpp + ConstraintSolver/b3TypedConstraint.cpp +) + +SET(Bullet3Dynamics_HDRS + b3CpuRigidBodyPipeline.h + ConstraintSolver/b3ContactSolverInfo.h + ConstraintSolver/b3FixedConstraint.h + ConstraintSolver/b3Generic6DofConstraint.h + ConstraintSolver/b3JacobianEntry.h + ConstraintSolver/b3PgsJacobiSolver.h + ConstraintSolver/b3Point2PointConstraint.h + ConstraintSolver/b3SolverBody.h + ConstraintSolver/b3SolverConstraint.h + ConstraintSolver/b3TypedConstraint.h + shared/b3ContactConstraint4.h + shared/b3ConvertConstraint4.h + shared/b3Inertia.h + shared/b3IntegrateTransforms.h +) + +ADD_LIBRARY(Bullet3Dynamics ${Bullet3Dynamics_SRCS} ${Bullet3Dynamics_HDRS}) +if (BUILD_SHARED_LIBS) + target_link_libraries(Bullet3Dynamics Bullet3Collision) +endif () +SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES SOVERSION ${BULLET_VERSION}) + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + #FILES_MATCHING requires CMake 2.6 + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS Bullet3Dynamics DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS Bullet3Dynamics + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN +".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(Bullet3Dynamics PROPERTIES PUBLIC_HEADER "${Bullet3Dynamics_HDRS}") + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h new file mode 100644 index 000000000000..7a12257b33c5 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h @@ -0,0 +1,159 @@ +/* +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 B3_CONTACT_SOLVER_INFO +#define B3_CONTACT_SOLVER_INFO + +#include "Bullet3Common/b3Scalar.h" + +enum b3SolverMode +{ + B3_SOLVER_RANDMIZE_ORDER = 1, + B3_SOLVER_FRICTION_SEPARATE = 2, + B3_SOLVER_USE_WARMSTARTING = 4, + B3_SOLVER_USE_2_FRICTION_DIRECTIONS = 16, + B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32, + B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64, + B3_SOLVER_CACHE_FRIENDLY = 128, + B3_SOLVER_SIMD = 256, + B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512, + B3_SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024 +}; + +struct b3ContactSolverInfoData +{ + + + b3Scalar m_tau; + b3Scalar m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. + b3Scalar m_friction; + b3Scalar m_timeStep; + b3Scalar m_restitution; + int m_numIterations; + b3Scalar m_maxErrorReduction; + b3Scalar m_sor; + b3Scalar m_erp;//used as Baumgarte factor + b3Scalar m_erp2;//used in Split Impulse + b3Scalar m_globalCfm;//constraint force mixing + int m_splitImpulse; + b3Scalar m_splitImpulsePenetrationThreshold; + b3Scalar m_splitImpulseTurnErp; + b3Scalar m_linearSlop; + b3Scalar m_warmstartingFactor; + + int m_solverMode; + int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; + b3Scalar m_maxGyroscopicForce; + b3Scalar m_singleAxisRollingFrictionThreshold; + + +}; + +struct b3ContactSolverInfo : public b3ContactSolverInfoData +{ + + + + inline b3ContactSolverInfo() + { + m_tau = b3Scalar(0.6); + m_damping = b3Scalar(1.0); + m_friction = b3Scalar(0.3); + m_timeStep = b3Scalar(1.f/60.f); + m_restitution = b3Scalar(0.); + m_maxErrorReduction = b3Scalar(20.); + m_numIterations = 10; + m_erp = b3Scalar(0.2); + m_erp2 = b3Scalar(0.8); + m_globalCfm = b3Scalar(0.); + m_sor = b3Scalar(1.); + m_splitImpulse = true; + m_splitImpulsePenetrationThreshold = -.04f; + m_splitImpulseTurnErp = 0.1f; + m_linearSlop = b3Scalar(0.0); + m_warmstartingFactor=b3Scalar(0.85); + //m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD | B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | B3_SOLVER_RANDMIZE_ORDER; + m_solverMode = B3_SOLVER_USE_WARMSTARTING | B3_SOLVER_SIMD;// | B3_SOLVER_RANDMIZE_ORDER; + m_restingContactRestitutionThreshold = 2;//unused as of 2.81 + m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit + m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag) + m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows. + } +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct b3ContactSolverInfoDoubleData +{ + double m_tau; + double m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. + double m_friction; + double m_timeStep; + double m_restitution; + double m_maxErrorReduction; + double m_sor; + double m_erp;//used as Baumgarte factor + double m_erp2;//used in Split Impulse + double m_globalCfm;//constraint force mixing + double m_splitImpulsePenetrationThreshold; + double m_splitImpulseTurnErp; + double m_linearSlop; + double m_warmstartingFactor; + double m_maxGyroscopicForce; + double m_singleAxisRollingFrictionThreshold; + + int m_numIterations; + int m_solverMode; + int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; + int m_splitImpulse; + char m_padding[4]; + +}; +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct b3ContactSolverInfoFloatData +{ + float m_tau; + float m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. + float m_friction; + float m_timeStep; + + float m_restitution; + float m_maxErrorReduction; + float m_sor; + float m_erp;//used as Baumgarte factor + + float m_erp2;//used in Split Impulse + float m_globalCfm;//constraint force mixing + float m_splitImpulsePenetrationThreshold; + float m_splitImpulseTurnErp; + + float m_linearSlop; + float m_warmstartingFactor; + float m_maxGyroscopicForce; + float m_singleAxisRollingFrictionThreshold; + + int m_numIterations; + int m_solverMode; + int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; + + int m_splitImpulse; + char m_padding[4]; +}; + + + +#endif //B3_CONTACT_SOLVER_INFO diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp new file mode 100644 index 000000000000..5e11e7493575 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp @@ -0,0 +1,108 @@ + +#include "b3FixedConstraint.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Common/b3TransformUtil.h" +#include + + +b3FixedConstraint::b3FixedConstraint(int rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB) +:b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE,rbA,rbB) +{ + m_pivotInA = frameInA.getOrigin(); + m_pivotInB = frameInB.getOrigin(); + m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse(); + +} + +b3FixedConstraint::~b3FixedConstraint () +{ +} + + +void b3FixedConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +{ + info->m_numConstraintRows = 6; + info->nub = 6; +} + +void b3FixedConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies) +{ + //fix the 3 linear degrees of freedom + + const b3Vector3& worldPosA = bodies[m_rbA].m_pos; + const b3Quaternion& worldOrnA = bodies[m_rbA].m_quat; + const b3Vector3& worldPosB= bodies[m_rbB].m_pos; + const b3Quaternion& worldOrnB = bodies[m_rbB].m_quat; + + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip+1] = 1; + info->m_J1linearAxis[2*info->rowskip+2] = 1; + + b3Vector3 a1 = b3QuatRotate(worldOrnA,m_pivotInA); + { + b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis); + b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip); + b3Vector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + if (info->m_J2linearAxis) + { + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip+1] = -1; + info->m_J2linearAxis[2*info->rowskip+2] = -1; + } + + b3Vector3 a2 = b3QuatRotate(worldOrnB,m_pivotInB); + + { + // b3Vector3 a2n = -a2; + b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis); + b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + // set right hand side for the linear dofs + b3Scalar k = info->fps * info->erp; + b3Vector3 linearError = k*(a2+worldPosB-a1-worldPosA); + int j; + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = linearError[j]; + //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + } + + //fix the 3 angular degrees of freedom + + int start_row = 3; + int s = info->rowskip; + int start_index = start_row * s; + + // 3 rows to make body rotations equal + info->m_J1angularAxis[start_index] = 1; + info->m_J1angularAxis[start_index + s + 1] = 1; + info->m_J1angularAxis[start_index + s*2+2] = 1; + if ( info->m_J2angularAxis) + { + info->m_J2angularAxis[start_index] = -1; + info->m_J2angularAxis[start_index + s+1] = -1; + info->m_J2angularAxis[start_index + s*2+2] = -1; + } + + + // set right hand side for the angular dofs + + b3Vector3 diff; + b3Scalar angle; + b3Quaternion qrelCur = worldOrnA *worldOrnB.inverse(); + + b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle); + diff*=-angle; + for (j=0; j<3; j++) + { + info->m_constraintError[(3+j)*info->rowskip] = k * diff[j]; + } + +} \ No newline at end of file diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h new file mode 100644 index 000000000000..e884a829129d --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h @@ -0,0 +1,35 @@ + +#ifndef B3_FIXED_CONSTRAINT_H +#define B3_FIXED_CONSTRAINT_H + +#include "b3TypedConstraint.h" + +B3_ATTRIBUTE_ALIGNED16(class) b3FixedConstraint : public b3TypedConstraint +{ + b3Vector3 m_pivotInA; + b3Vector3 m_pivotInB; + b3Quaternion m_relTargetAB; + +public: + b3FixedConstraint(int rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB); + + virtual ~b3FixedConstraint(); + + + virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + + virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies); + + virtual void setParam(int num, b3Scalar value, int axis = -1) + { + b3Assert(0); + } + virtual b3Scalar getParam(int num, int axis = -1) const + { + b3Assert(0); + return 0.f; + } + +}; + +#endif //B3_FIXED_CONSTRAINT_H diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp new file mode 100644 index 000000000000..3ae2922e586f --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp @@ -0,0 +1,807 @@ +/* +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@yahoo.com +http://gimpact.sf.net +*/ + +#include "b3Generic6DofConstraint.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + +#include "Bullet3Common/b3TransformUtil.h" +#include "Bullet3Common/b3TransformUtil.h" +#include + + + +#define D6_USE_OBSOLETE_METHOD false +#define D6_USE_FRAME_OFFSET true + + + + + + +b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies) +: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB) +, m_frameInA(frameInA) +, m_frameInB(frameInB), +m_useLinearReferenceFrameA(useLinearReferenceFrameA), +m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), +m_flags(0) +{ + calculateTransforms(bodies); +} + + + + + + +#define GENERIC_D6_DISABLE_WARMSTARTING 1 + + + +b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index); +b3Scalar btGetMatrixElem(const b3Matrix3x3& 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 b3Matrix3x3& mat,b3Vector3& xyz); +bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& 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 + // + + b3Scalar fi = btGetMatrixElem(mat,2); + if (fi < b3Scalar(1.0f)) + { + if (fi > b3Scalar(-1.0f)) + { + xyz[0] = b3Atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8)); + xyz[1] = b3Asin(btGetMatrixElem(mat,2)); + xyz[2] = b3Atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + return true; + } + else + { + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + xyz[0] = -b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = -B3_HALF_PI; + xyz[2] = b3Scalar(0.0); + return false; + } + } + else + { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz[0] = b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + xyz[1] = B3_HALF_PI; + xyz[2] = 0.0; + } + return false; +} + +//////////////////////////// b3RotationalLimitMotor //////////////////////////////////// + +int b3RotationalLimitMotor::testLimitValue(b3Scalar 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; + if(m_currentLimitError>B3_PI) + m_currentLimitError-=B3_2_PI; + else if(m_currentLimitError<-B3_PI) + m_currentLimitError+=B3_2_PI; + return 1; + } + else if (test_value> m_hiLimit) + { + m_currentLimit = 2;//High limit violation + m_currentLimitError = test_value - m_hiLimit; + if(m_currentLimitError>B3_PI) + m_currentLimitError-=B3_2_PI; + else if(m_currentLimitError<-B3_PI) + m_currentLimitError+=B3_2_PI; + return 2; + }; + + m_currentLimit = 0;//Free from violation + return 0; + +} + + + + +//////////////////////////// End b3RotationalLimitMotor //////////////////////////////////// + + + + +//////////////////////////// b3TranslationalLimitMotor //////////////////////////////////// + + +int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_value) +{ + b3Scalar loLimit = m_lowerLimit[limitIndex]; + b3Scalar hiLimit = m_upperLimit[limitIndex]; + if(loLimit > hiLimit) + { + m_currentLimit[limitIndex] = 0;//Free from violation + m_currentLimitError[limitIndex] = b3Scalar(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] = b3Scalar(0.f); + return 0; +} + + + +//////////////////////////// b3TranslationalLimitMotor //////////////////////////////////// + +void b3Generic6DofConstraint::calculateAngleInfo() +{ + b3Matrix3x3 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. + b3Vector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); + b3Vector3 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(); + +} + +static b3Transform getCenterOfMassTransform(const b3RigidBodyData& body) +{ + b3Transform tr(body.m_quat,body.m_pos); + return tr; +} + +void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyData* bodies) +{ + b3Transform transA; + b3Transform transB; + transA = getCenterOfMassTransform(bodies[m_rbA]); + transB = getCenterOfMassTransform(bodies[m_rbB]); + calculateTransforms(transA,transB,bodies); +} + +void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies) +{ + m_calculatedTransformA = transA * m_frameInA; + m_calculatedTransformB = transB * m_frameInB; + calculateLinearInfo(); + calculateAngleInfo(); + if(m_useOffsetForConstraintFrame) + { // get weight factors depending on masses + b3Scalar miA = bodies[m_rbA].m_invMass; + b3Scalar miB = bodies[m_rbB].m_invMass; + m_hasStaticBody = (miA < B3_EPSILON) || (miB < B3_EPSILON); + b3Scalar miS = miA + miB; + if(miS > b3Scalar(0.f)) + { + m_factA = miB / miS; + } + else + { + m_factA = b3Scalar(0.5f); + } + m_factB = b3Scalar(1.0f) - m_factA; + } +} + + + + + + + +bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index) +{ + b3Scalar angle = m_calculatedAxisAngleDiff[axis_index]; + angle = b3AdjustAngleToLimits(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 b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +{ + //prepare constraint + calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies); + 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--; + } + } +// printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows); +} + +void b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +{ + //pre-allocate all 6 + info->m_numConstraintRows = 6; + info->nub = 0; +} + + +void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies) +{ + + b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]); + b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]); + const b3Vector3& linVelA = bodies[m_rbA].m_linVel; + const b3Vector3& linVelB = bodies[m_rbB].m_linVel; + const b3Vector3& angVelA = bodies[m_rbA].m_angVel; + const b3Vector3& angVelB = bodies[m_rbB].m_angVel; + + if(m_useOffsetForConstraintFrame) + { // for stability better to solve angular limits first + int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); + setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); + } + else + { // leave old version for compatibility + int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB); + setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB); + } + +} + + +void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies) +{ + + //prepare constraint + calculateTransforms(transA,transB,bodies); + + int i; + for (i=0;i<3 ;i++ ) + { + testAngularLimitMotor(i); + } + + if(m_useOffsetForConstraintFrame) + { // for stability better to solve angular limits first + int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); + setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); + } + else + { // leave old version for compatibility + int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB); + setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB); + } +} + + + +int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB) +{ +// int row = 0; + //solve linear limits + b3RotationalLimitMotor limot; + for (int i=0;i<3 ;i++ ) + { + if(m_linearLimits.needApplyForce(i)) + { // re-use rotational motor code + limot.m_bounce = b3Scalar(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_hiLimit = m_linearLimits.m_upperLimit[i]; + limot.m_limitSoftness = m_linearLimits.m_limitSoftness; + limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; + limot.m_maxLimitForce = b3Scalar(0.f); + limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; + limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; + b3Vector3 axis = m_calculatedTransformA.getBasis().getColumn(i); + int flags = m_flags >> (i * B3_6DOF_FLAGS_AXIS_SHIFT); + limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0]; + limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; + limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp; + if(m_useOffsetForConstraintFrame) + { + int indx1 = (i + 1) % 3; + int indx2 = (i + 2) % 3; + int rotAllowed = 1; // rotations around orthos to current axis + if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit) + { + rotAllowed = 0; + } + row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed); + } + else + { + row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0); + } + } + } + return row; +} + + + +int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2 *info, int row_offset, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB) +{ + b3Generic6DofConstraint * d6constraint = this; + int row = row_offset; + //solve angular limits + for (int i=0;i<3 ;i++ ) + { + if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) + { + b3Vector3 axis = d6constraint->getAxis(i); + int flags = m_flags >> ((i + 3) * B3_6DOF_FLAGS_AXIS_SHIFT); + if(!(flags & B3_6DOF_FLAGS_CFM_NORM)) + { + m_angularLimits[i].m_normalCFM = info->cfm[0]; + } + if(!(flags & B3_6DOF_FLAGS_CFM_STOP)) + { + m_angularLimits[i].m_stopCFM = info->cfm[0]; + } + if(!(flags & B3_6DOF_FLAGS_ERP_STOP)) + { + m_angularLimits[i].m_stopERP = info->erp; + } + row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i), + transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1); + } + } + + return row; +} + + + + +void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep) +{ + (void)timeStep; + +} + + +void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB,const b3RigidBodyData* bodies) +{ + m_frameInA = frameA; + m_frameInB = frameB; + + calculateTransforms(bodies); +} + + + +b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const +{ + return m_calculatedAxis[axis_index]; +} + + +b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const +{ + return m_calculatedLinearDiff[axisIndex]; +} + + +b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const +{ + return m_calculatedAxisAngleDiff[axisIndex]; +} + + + +void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies) +{ + b3Scalar imA = bodies[m_rbA].m_invMass; + b3Scalar imB = bodies[m_rbB].m_invMass; + b3Scalar weight; + if(imB == b3Scalar(0.0)) + { + weight = b3Scalar(1.0); + } + else + { + weight = imA / (imA + imB); + } + const b3Vector3& pA = m_calculatedTransformA.getOrigin(); + const b3Vector3& pB = m_calculatedTransformB.getOrigin(); + m_AnchorPos = pA * weight + pB * (b3Scalar(1.0) - weight); + return; +} + + + +void b3Generic6DofConstraint::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 b3Generic6DofConstraint::get_limit_motor_info2( + b3RotationalLimitMotor * limot, + const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB, + b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational,int rotAllowed) +{ + int srow = row * info->rowskip; + bool 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 + b3Scalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; + b3Scalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis; + if (J1) + { + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + } + if (J2) + { + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + } + if((!rotational)) + { + if (m_useOffsetForConstraintFrame) + { + b3Vector3 tmpA, tmpB, relA, relB; + // get vector from bodyB to frameB in WCS + relB = m_calculatedTransformB.getOrigin() - transB.getOrigin(); + // get its projection to constraint axis + b3Vector3 projB = ax1 * relB.dot(ax1); + // get vector directed from bodyB to constraint axis (and orthogonal to it) + b3Vector3 orthoB = relB - projB; + // same for bodyA + relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); + b3Vector3 projA = ax1 * relA.dot(ax1); + b3Vector3 orthoA = relA - projA; + // get desired offset between frames A and B along constraint axis + b3Scalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError; + // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis + b3Vector3 totalDist = projA + ax1 * desiredOffs - projB; + // get offset vectors relA and relB + relA = orthoA + totalDist * m_factA; + relB = orthoB - totalDist * m_factB; + tmpA = relA.cross(ax1); + tmpB = relB.cross(ax1); + if(m_hasStaticBody && (!rotAllowed)) + { + tmpA *= m_factA; + tmpB *= m_factB; + } + int i; + for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i]; + } else + { + b3Vector3 ltd; // Linear Torque Decoupling vector + b3Vector3 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 = false; + info->m_constraintError[srow] = b3Scalar(0.f); + if (powered) + { + info->cfm[srow] = limot->m_normalCFM; + if(!limit) + { + b3Scalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity; + + b3Scalar mot_fact = getMotorFactor( limot->m_currentPosition, + limot->m_loLimit, + limot->m_hiLimit, + tag_vel, + info->fps * limot->m_stopERP); + info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; + } + } + if(limit) + { + b3Scalar k = info->fps * limot->m_stopERP; + if(!rotational) + { + info->m_constraintError[srow] += k * limot->m_currentLimitError; + } + else + { + info->m_constraintError[srow] += -k * limot->m_currentLimitError; + } + info->cfm[srow] = limot->m_stopCFM; + if (limot->m_loLimit == limot->m_hiLimit) + { // limited low and high simultaneously + info->m_lowerLimit[srow] = -B3_INFINITY; + info->m_upperLimit[srow] = B3_INFINITY; + } + else + { + if (limit == 1) + { + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = B3_INFINITY; + } + else + { + info->m_lowerLimit[srow] = -B3_INFINITY; + info->m_upperLimit[srow] = 0; + } + // deal with bounce + if (limot->m_bounce > 0) + { + // calculate joint velocity + b3Scalar 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) + { + b3Scalar newc = -limot->m_bounce* vel; + if (newc > info->m_constraintError[srow]) + info->m_constraintError[srow] = newc; + } + } + else + { + if (vel > 0) + { + b3Scalar newc = -limot->m_bounce * vel; + if (newc < info->m_constraintError[srow]) + info->m_constraintError[srow] = newc; + } + } + } + } + } + return 1; + } + else return 0; +} + + + + + + + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///If no axis is provided, it uses the default axis for this constraint. +void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis) +{ + if((axis >= 0) && (axis < 3)) + { + switch(num) + { + case B3_CONSTRAINT_STOP_ERP : + m_linearLimits.m_stopERP[axis] = value; + m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); + break; + case B3_CONSTRAINT_STOP_CFM : + m_linearLimits.m_stopCFM[axis] = value; + m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); + break; + case B3_CONSTRAINT_CFM : + m_linearLimits.m_normalCFM[axis] = value; + m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); + break; + default : + b3AssertConstrParams(0); + } + } + else if((axis >=3) && (axis < 6)) + { + switch(num) + { + case B3_CONSTRAINT_STOP_ERP : + m_angularLimits[axis - 3].m_stopERP = value; + m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); + break; + case B3_CONSTRAINT_STOP_CFM : + m_angularLimits[axis - 3].m_stopCFM = value; + m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); + break; + case B3_CONSTRAINT_CFM : + m_angularLimits[axis - 3].m_normalCFM = value; + m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT); + break; + default : + b3AssertConstrParams(0); + } + } + else + { + b3AssertConstrParams(0); + } +} + + ///return the local value of parameter +b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const +{ + b3Scalar retVal = 0; + if((axis >= 0) && (axis < 3)) + { + switch(num) + { + case B3_CONSTRAINT_STOP_ERP : + b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_linearLimits.m_stopERP[axis]; + break; + case B3_CONSTRAINT_STOP_CFM : + b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_linearLimits.m_stopCFM[axis]; + break; + case B3_CONSTRAINT_CFM : + b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_linearLimits.m_normalCFM[axis]; + break; + default : + b3AssertConstrParams(0); + } + } + else if((axis >=3) && (axis < 6)) + { + switch(num) + { + case B3_CONSTRAINT_STOP_ERP : + b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_angularLimits[axis - 3].m_stopERP; + break; + case B3_CONSTRAINT_STOP_CFM : + b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_angularLimits[axis - 3].m_stopCFM; + break; + case B3_CONSTRAINT_CFM : + b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT))); + retVal = m_angularLimits[axis - 3].m_normalCFM; + break; + default : + b3AssertConstrParams(0); + } + } + else + { + b3AssertConstrParams(0); + } + return retVal; +} + + + +void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1,const b3Vector3& axis2, const b3RigidBodyData* bodies) +{ + b3Vector3 zAxis = axis1.normalized(); + b3Vector3 yAxis = axis2.normalized(); + b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system + + b3Transform frameInW; + frameInW.setIdentity(); + frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], + xAxis[1], yAxis[1], zAxis[1], + xAxis[2], yAxis[2], zAxis[2]); + + // now get constraint frame in local coordinate systems + m_frameInA = getCenterOfMassTransform(bodies[m_rbA]).inverse() * frameInW; + m_frameInB = getCenterOfMassTransform(bodies[m_rbB]).inverse() * frameInW; + + calculateTransforms(bodies); +} diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h new file mode 100644 index 000000000000..169b1b94ade6 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h @@ -0,0 +1,550 @@ +/* +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. +*/ + +/// 2009 March: b3Generic6DofConstraint refactored by Roman Ponomarev +/// Added support for generic constraint solver through getInfo1/getInfo2 methods + +/* +2007-09-09 +b3Generic6DofConstraint Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + + +#ifndef B3_GENERIC_6DOF_CONSTRAINT_H +#define B3_GENERIC_6DOF_CONSTRAINT_H + +#include "Bullet3Common/b3Vector3.h" +#include "b3JacobianEntry.h" +#include "b3TypedConstraint.h" + +struct b3RigidBodyData; + + + + +//! Rotation Limit structure for generic joints +class b3RotationalLimitMotor +{ +public: + //! limit_parameters + //!@{ + b3Scalar m_loLimit;//!< joint limit + b3Scalar m_hiLimit;//!< joint limit + b3Scalar m_targetVelocity;//!< target motor velocity + b3Scalar m_maxMotorForce;//!< max force on motor + b3Scalar m_maxLimitForce;//!< max force on limit + b3Scalar m_damping;//!< Damping. + b3Scalar m_limitSoftness;//! Relaxation factor + b3Scalar m_normalCFM;//!< Constraint force mixing factor + b3Scalar m_stopERP;//!< Error tolerance factor when joint is at limit + b3Scalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit + b3Scalar m_bounce;//!< restitution factor + bool m_enableMotor; + + //!@} + + //! temp_variables + //!@{ + b3Scalar m_currentLimitError;//! How much is violated this limit + b3Scalar m_currentPosition; //! current value of angle + int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit + b3Scalar m_accumulatedImpulse; + //!@} + + b3RotationalLimitMotor() + { + m_accumulatedImpulse = 0.f; + m_targetVelocity = 0; + m_maxMotorForce = 6.0f; + m_maxLimitForce = 300.0f; + m_loLimit = 1.0f; + m_hiLimit = -1.0f; + m_normalCFM = 0.f; + m_stopERP = 0.2f; + m_stopCFM = 0.f; + m_bounce = 0.0f; + m_damping = 1.0f; + m_limitSoftness = 0.5f; + m_currentLimit = 0; + m_currentLimitError = 0; + m_enableMotor = false; + } + + b3RotationalLimitMotor(const b3RotationalLimitMotor & limot) + { + m_targetVelocity = limot.m_targetVelocity; + m_maxMotorForce = limot.m_maxMotorForce; + m_limitSoftness = limot.m_limitSoftness; + m_loLimit = limot.m_loLimit; + m_hiLimit = limot.m_hiLimit; + m_normalCFM = limot.m_normalCFM; + m_stopERP = limot.m_stopERP; + m_stopCFM = limot.m_stopCFM; + m_bounce = limot.m_bounce; + m_currentLimit = limot.m_currentLimit; + m_currentLimitError = limot.m_currentLimitError; + m_enableMotor = limot.m_enableMotor; + } + + + + //! Is limited + bool isLimited() + { + if(m_loLimit > m_hiLimit) return false; + return true; + } + + //! Need apply correction + bool needApplyTorques() + { + if(m_currentLimit == 0 && m_enableMotor == false) return false; + return true; + } + + //! calculates error + /*! + calculates m_currentLimit and m_currentLimitError. + */ + int testLimitValue(b3Scalar test_value); + + //! apply the correction impulses for two bodies + b3Scalar solveAngularLimits(b3Scalar timeStep,b3Vector3& axis, b3Scalar jacDiagABInv,b3RigidBodyData * body0, b3RigidBodyData * body1); + +}; + + + +class b3TranslationalLimitMotor +{ +public: + b3Vector3 m_lowerLimit;//!< the constraint lower limits + b3Vector3 m_upperLimit;//!< the constraint upper limits + b3Vector3 m_accumulatedImpulse; + //! Linear_Limit_parameters + //!@{ + b3Vector3 m_normalCFM;//!< Constraint force mixing factor + b3Vector3 m_stopERP;//!< Error tolerance factor when joint is at limit + b3Vector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit + b3Vector3 m_targetVelocity;//!< target motor velocity + b3Vector3 m_maxMotorForce;//!< max force on motor + b3Vector3 m_currentLimitError;//! How much is violated this limit + b3Vector3 m_currentLinearDiff;//! Current relative offset of constraint frames + b3Scalar m_limitSoftness;//!< Softness for linear limit + b3Scalar m_damping;//!< Damping for linear limit + b3Scalar m_restitution;//! Bounce parameter for linear limit + //!@} + bool m_enableMotor[3]; + int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit + + b3TranslationalLimitMotor() + { + m_lowerLimit.setValue(0.f,0.f,0.f); + m_upperLimit.setValue(0.f,0.f,0.f); + m_accumulatedImpulse.setValue(0.f,0.f,0.f); + m_normalCFM.setValue(0.f, 0.f, 0.f); + m_stopERP.setValue(0.2f, 0.2f, 0.2f); + m_stopCFM.setValue(0.f, 0.f, 0.f); + + m_limitSoftness = 0.7f; + m_damping = b3Scalar(1.0f); + m_restitution = b3Scalar(0.5f); + for(int i=0; i < 3; i++) + { + m_enableMotor[i] = false; + m_targetVelocity[i] = b3Scalar(0.f); + m_maxMotorForce[i] = b3Scalar(0.f); + } + } + + b3TranslationalLimitMotor(const b3TranslationalLimitMotor & other ) + { + m_lowerLimit = other.m_lowerLimit; + m_upperLimit = other.m_upperLimit; + m_accumulatedImpulse = other.m_accumulatedImpulse; + + m_limitSoftness = other.m_limitSoftness ; + m_damping = other.m_damping; + m_restitution = other.m_restitution; + m_normalCFM = other.m_normalCFM; + m_stopERP = other.m_stopERP; + m_stopCFM = other.m_stopCFM; + + for(int i=0; i < 3; i++) + { + m_enableMotor[i] = other.m_enableMotor[i]; + m_targetVelocity[i] = other.m_targetVelocity[i]; + m_maxMotorForce[i] = other.m_maxMotorForce[i]; + } + } + + //! Test limit + /*! + - free means upper < lower, + - locked means upper == lower + - limited means upper > lower + - limitIndex: first 3 are linear, next 3 are angular + */ + inline bool isLimited(int limitIndex) + { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + inline bool needApplyForce(int limitIndex) + { + if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false; + return true; + } + int testLimitValue(int limitIndex, b3Scalar test_value); + + + b3Scalar solveLinearAxis( + b3Scalar timeStep, + b3Scalar jacDiagABInv, + b3RigidBodyData& body1,const b3Vector3 &pointInA, + b3RigidBodyData& body2,const b3Vector3 &pointInB, + int limit_index, + const b3Vector3 & axis_normal_on_a, + const b3Vector3 & anchorPos); + + +}; + +enum b36DofFlags +{ + B3_6DOF_FLAGS_CFM_NORM = 1, + B3_6DOF_FLAGS_CFM_STOP = 2, + B3_6DOF_FLAGS_ERP_STOP = 4 +}; +#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis + + +/// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space +/*! +b3Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'. +currently this limit supports rotational motors
+
    +
  • For Linear limits, use b3Generic6DofConstraint.setLinearUpperLimit, b3Generic6DofConstraint.setLinearLowerLimit. You can set the parameters with the b3TranslationalLimitMotor structure accsesible through the b3Generic6DofConstraint.getTranslationalLimitMotor method. +At this moment translational motors are not supported. May be in the future.
    • + +
  • For Angular limits, use the b3RotationalLimitMotor structure for configuring the limit. +This is accessible through b3Generic6DofConstraint.getLimitMotor method, +This brings support for limit parameters and motors.
    • + +
  • Angulars limits have these possible ranges: + + + + + + + + + + + + + + + + + + +
    AXISMIN ANGLEMAX ANGLE
    X-PIPI
    Y-PI/2PI/2
    Z-PIPI
    +
    • +
+ +*/ +B3_ATTRIBUTE_ALIGNED16(class) b3Generic6DofConstraint : public b3TypedConstraint +{ +protected: + + //! relative_frames + //!@{ + b3Transform m_frameInA;//!< the constraint space w.r.t body A + b3Transform m_frameInB;//!< the constraint space w.r.t body B + //!@} + + //! Jacobians + //!@{ +// b3JacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints +// b3JacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints + //!@} + + //! Linear_Limit_parameters + //!@{ + b3TranslationalLimitMotor m_linearLimits; + //!@} + + + //! hinge_parameters + //!@{ + b3RotationalLimitMotor m_angularLimits[3]; + //!@} + + +protected: + //! temporal variables + //!@{ + b3Transform m_calculatedTransformA; + b3Transform m_calculatedTransformB; + b3Vector3 m_calculatedAxisAngleDiff; + b3Vector3 m_calculatedAxis[3]; + b3Vector3 m_calculatedLinearDiff; + b3Scalar m_timeStep; + b3Scalar m_factA; + b3Scalar m_factB; + bool m_hasStaticBody; + + b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes + + bool m_useLinearReferenceFrameA; + bool m_useOffsetForConstraintFrame; + + int m_flags; + + //!@} + + b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other) + { + b3Assert(0); + (void) other; + return *this; + } + + + int setAngularLimits(b3ConstraintInfo2 *info, int row_offset,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB); + + int setLinearLimits(b3ConstraintInfo2 *info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB); + + + // tests linear limits + void calculateLinearInfo(); + + //! calcs the euler angles between the two bodies. + void calculateAngleInfo(); + + + +public: + + B3_DECLARE_ALIGNED_ALLOCATOR(); + + b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB ,bool useLinearReferenceFrameA,const b3RigidBodyData* bodies); + + //! Calcs global transform of the offsets + /*! + Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. + \sa b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo + */ + void calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyData* bodies); + + void calculateTransforms(const b3RigidBodyData* bodies); + + //! Gets the global transform of the offset for body A + /*! + \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo. + */ + const b3Transform & getCalculatedTransformA() const + { + return m_calculatedTransformA; + } + + //! Gets the global transform of the offset for body B + /*! + \sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo. + */ + const b3Transform & getCalculatedTransformB() const + { + return m_calculatedTransformB; + } + + const b3Transform & getFrameOffsetA() const + { + return m_frameInA; + } + + const b3Transform & getFrameOffsetB() const + { + return m_frameInB; + } + + + b3Transform & getFrameOffsetA() + { + return m_frameInA; + } + + b3Transform & getFrameOffsetB() + { + return m_frameInB; + } + + + + virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + + void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + + virtual void getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyData* bodies); + + void getInfo2NonVirtual (b3ConstraintInfo2* info,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyData* bodies); + + + void updateRHS(b3Scalar timeStep); + + //! Get the rotation axis in global coordinates + b3Vector3 getAxis(int axis_index) const; + + //! Get the relative Euler angle + /*! + \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. + */ + b3Scalar getAngle(int axis_index) const; + + //! Get the relative position of the constraint pivot + /*! + \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. + */ + b3Scalar getRelativePivotPosition(int axis_index) const; + + void setFrames(const b3Transform & frameA, const b3Transform & frameB, const b3RigidBodyData* bodies); + + //! Test angular limit. + /*! + Calculates angular correction and returns true if limit needs to be corrected. + \pre b3Generic6DofConstraint::calculateTransforms() must be called previously. + */ + bool testAngularLimitMotor(int axis_index); + + void setLinearLowerLimit(const b3Vector3& linearLower) + { + m_linearLimits.m_lowerLimit = linearLower; + } + + void getLinearLowerLimit(b3Vector3& linearLower) + { + linearLower = m_linearLimits.m_lowerLimit; + } + + void setLinearUpperLimit(const b3Vector3& linearUpper) + { + m_linearLimits.m_upperLimit = linearUpper; + } + + void getLinearUpperLimit(b3Vector3& linearUpper) + { + linearUpper = m_linearLimits.m_upperLimit; + } + + void setAngularLowerLimit(const b3Vector3& angularLower) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]); + } + + void getAngularLowerLimit(b3Vector3& angularLower) + { + for(int i = 0; i < 3; i++) + angularLower[i] = m_angularLimits[i].m_loLimit; + } + + void setAngularUpperLimit(const b3Vector3& angularUpper) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]); + } + + void getAngularUpperLimit(b3Vector3& angularUpper) + { + for(int i = 0; i < 3; i++) + angularUpper[i] = m_angularLimits[i].m_hiLimit; + } + + //! Retrieves the angular limit informacion + b3RotationalLimitMotor * getRotationalLimitMotor(int index) + { + return &m_angularLimits[index]; + } + + //! Retrieves the limit informacion + b3TranslationalLimitMotor * getTranslationalLimitMotor() + { + return &m_linearLimits; + } + + //first 3 are linear, next 3 are angular + void setLimit(int axis, b3Scalar lo, b3Scalar hi) + { + if(axis<3) + { + m_linearLimits.m_lowerLimit[axis] = lo; + m_linearLimits.m_upperLimit[axis] = hi; + } + else + { + lo = b3NormalizeAngle(lo); + hi = b3NormalizeAngle(hi); + m_angularLimits[axis-3].m_loLimit = lo; + m_angularLimits[axis-3].m_hiLimit = hi; + } + } + + //! Test limit + /*! + - free means upper < lower, + - locked means upper == lower + - limited means upper > lower + - limitIndex: first 3 are linear, next 3 are angular + */ + bool isLimited(int limitIndex) + { + if(limitIndex<3) + { + return m_linearLimits.isLimited(limitIndex); + + } + return m_angularLimits[limitIndex-3].isLimited(); + } + + virtual void calcAnchorPos(const b3RigidBodyData* bodies); // overridable + + int get_limit_motor_info2( b3RotationalLimitMotor * limot, + const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB, + b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false); + + // access for UseFrameOffset + bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; } + void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } + + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///If no axis is provided, it uses the default axis for this constraint. + virtual void setParam(int num, b3Scalar value, int axis = -1); + ///return the local value of parameter + virtual b3Scalar getParam(int num, int axis = -1) const; + + void setAxis( const b3Vector3& axis1, const b3Vector3& axis2,const b3RigidBodyData* bodies); + + + + +}; + + + + + +#endif //B3_GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h new file mode 100644 index 000000000000..a55168eb38f7 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h @@ -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. +*/ + +#ifndef B3_JACOBIAN_ENTRY_H +#define B3_JACOBIAN_ENTRY_H + +#include "Bullet3Common/b3Matrix3x3.h" + + +//notes: +// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components +// which makes the b3JacobianEntry memory layout 16 bytes +// if you only are interested in angular part, just feed massInvA and massInvB zero + +/// Jacobian entry is an abstraction that allows to describe constraints +/// it can be used in combination with a constraint solver +/// Can be used to relate the effect of an impulse to the constraint error +B3_ATTRIBUTE_ALIGNED16(class) b3JacobianEntry +{ +public: + b3JacobianEntry() {}; + //constraint between two different rigidbodies + b3JacobianEntry( + const b3Matrix3x3& world2A, + const b3Matrix3x3& world2B, + const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, + const b3Vector3& jointAxis, + const b3Vector3& inertiaInvA, + const b3Scalar massInvA, + const b3Vector3& inertiaInvB, + const b3Scalar massInvB) + :m_linearJointAxis(jointAxis) + { + m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis)); + m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); + + b3Assert(m_Adiag > b3Scalar(0.0)); + } + + //angular constraint between two different rigidbodies + b3JacobianEntry(const b3Vector3& jointAxis, + const b3Matrix3x3& world2A, + const b3Matrix3x3& world2B, + const b3Vector3& inertiaInvA, + const b3Vector3& inertiaInvB) + :m_linearJointAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.))) + { + m_aJ= world2A*jointAxis; + m_bJ = world2B*-jointAxis; + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + b3Assert(m_Adiag > b3Scalar(0.0)); + } + + //angular constraint between two different rigidbodies + b3JacobianEntry(const b3Vector3& axisInA, + const b3Vector3& axisInB, + const b3Vector3& inertiaInvA, + const b3Vector3& inertiaInvB) + : m_linearJointAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.))) + , m_aJ(axisInA) + , m_bJ(-axisInB) + { + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + b3Assert(m_Adiag > b3Scalar(0.0)); + } + + //constraint on one rigidbody + b3JacobianEntry( + const b3Matrix3x3& world2A, + const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, + const b3Vector3& jointAxis, + const b3Vector3& inertiaInvA, + const b3Scalar massInvA) + :m_linearJointAxis(jointAxis) + { + m_aJ= world2A*(rel_pos1.cross(jointAxis)); + m_bJ = world2A*(rel_pos2.cross(-jointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); + + b3Assert(m_Adiag > b3Scalar(0.0)); + } + + b3Scalar getDiagonal() const { return m_Adiag; } + + // for two constraints on the same rigidbody (for example vehicle friction) + b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const + { + const b3JacobianEntry& jacA = *this; + b3Scalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); + b3Scalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ); + return lin + ang; + } + + + + // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) + b3Scalar getNonDiagonal(const b3JacobianEntry& jacB,const b3Scalar massInvA,const b3Scalar massInvB) const + { + const b3JacobianEntry& jacA = *this; + b3Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; + b3Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; + b3Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; + b3Vector3 lin0 = massInvA * lin ; + b3Vector3 lin1 = massInvB * lin; + b3Vector3 sum = ang0+ang1+lin0+lin1; + return sum[0]+sum[1]+sum[2]; + } + + b3Scalar getRelativeVelocity(const b3Vector3& linvelA,const b3Vector3& angvelA,const b3Vector3& linvelB,const b3Vector3& angvelB) + { + b3Vector3 linrel = linvelA - linvelB; + b3Vector3 angvela = angvelA * m_aJ; + b3Vector3 angvelb = angvelB * m_bJ; + linrel *= m_linearJointAxis; + angvela += angvelb; + angvela += linrel; + b3Scalar rel_vel2 = angvela[0]+angvela[1]+angvela[2]; + return rel_vel2 + B3_EPSILON; + } +//private: + + b3Vector3 m_linearJointAxis; + b3Vector3 m_aJ; + b3Vector3 m_bJ; + b3Vector3 m_0MinvJt; + b3Vector3 m_1MinvJt; + //Optimization: can be stored in the w/last component of one of the vectors + b3Scalar m_Adiag; + +}; + +#endif //B3_JACOBIAN_ENTRY_H diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp new file mode 100644 index 000000000000..de729d4556a0 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp @@ -0,0 +1,1815 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2012 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. +*/ + +//enable B3_SOLVER_DEBUG if you experience solver crashes +//#define B3_SOLVER_DEBUG +//#define COMPUTE_IMPULSE_DENOM 1 +//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. + +//#define DISABLE_JOINTS + +#include "b3PgsJacobiSolver.h" +#include "Bullet3Common/b3MinMax.h" +#include "b3TypedConstraint.h" +#include +#include "Bullet3Common/b3StackAlloc.h" + +//#include "b3SolverBody.h" +//#include "b3SolverConstraint.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include //for memset +//#include "../../dynamics/basic_demo/Stubs/AdlContact4.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" + + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + +static b3Transform getWorldTransform(b3RigidBodyData* rb) +{ + b3Transform newTrans; + newTrans.setOrigin(rb->m_pos); + newTrans.setRotation(rb->m_quat); + return newTrans; +} + +static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaData* inertia) +{ + return inertia->m_invInertiaWorld; +} + + + +static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb) +{ + return rb->m_linVel; +} + +static const b3Vector3& getAngularVelocity(b3RigidBodyData* rb) +{ + return rb->m_angVel; +} + +static b3Vector3 getVelocityInLocalPoint(b3RigidBodyData* rb, const b3Vector3& rel_pos) +{ + //we also calculate lin/ang velocity for kinematic objects + return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos); + +} + +struct b3ContactPoint +{ + b3Vector3 m_positionWorldOnA; + b3Vector3 m_positionWorldOnB; + b3Vector3 m_normalWorldOnB; + b3Scalar m_appliedImpulse; + b3Scalar m_distance; + b3Scalar m_combinedRestitution; + + ///information related to friction + b3Scalar m_combinedFriction; + b3Vector3 m_lateralFrictionDir1; + b3Vector3 m_lateralFrictionDir2; + b3Scalar m_appliedImpulseLateral1; + b3Scalar m_appliedImpulseLateral2; + b3Scalar m_combinedRollingFriction; + b3Scalar m_contactMotion1; + b3Scalar m_contactMotion2; + b3Scalar m_contactCFM1; + b3Scalar m_contactCFM2; + + bool m_lateralFrictionInitialized; + + b3Vector3 getPositionWorldOnA() + { + return m_positionWorldOnA; + } + b3Vector3 getPositionWorldOnB() + { + return m_positionWorldOnB; + } + b3Scalar getDistance() + { + return m_distance; + } +}; + +void getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& pointOut) +{ + pointOut.m_appliedImpulse = 0.f; + pointOut.m_appliedImpulseLateral1 = 0.f; + pointOut.m_appliedImpulseLateral2 = 0.f; + pointOut.m_combinedFriction = contact->getFrictionCoeff(); + pointOut.m_combinedRestitution = contact->getRestituitionCoeff(); + pointOut.m_combinedRollingFriction = 0.f; + pointOut.m_contactCFM1 = 0.f; + pointOut.m_contactCFM2 = 0.f; + pointOut.m_contactMotion1 = 0.f; + pointOut.m_contactMotion2 = 0.f; + pointOut.m_distance = contact->getPenetration(contactIndex);//??0.01f + b3Vector3 normalOnB = contact->m_worldNormalOnB; + normalOnB.normalize();//is this needed? + + b3Vector3 l1,l2; + b3PlaneSpace1(normalOnB,l1,l2); + + pointOut.m_normalWorldOnB = normalOnB; + //printf("normalOnB = %f,%f,%f\n",normalOnB.getX(),normalOnB.getY(),normalOnB.getZ()); + pointOut.m_lateralFrictionDir1 = l1; + pointOut.m_lateralFrictionDir2 = l2; + pointOut.m_lateralFrictionInitialized = true; + + + b3Vector3 worldPosB = contact->m_worldPosB[contactIndex]; + pointOut.m_positionWorldOnB = worldPosB; + pointOut.m_positionWorldOnA = worldPosB+normalOnB*pointOut.m_distance; +} + +int getNumContacts(b3Contact4* contact) +{ + return contact->getNPoints(); +} + +b3PgsJacobiSolver::b3PgsJacobiSolver(bool usePgs) +:m_usePgs(usePgs), +m_numSplitImpulseRecoveries(0), +m_btSeed2(0) +{ + +} + +b3PgsJacobiSolver::~b3PgsJacobiSolver() +{ +} + +void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints) +{ + b3ContactSolverInfo infoGlobal; + infoGlobal.m_splitImpulse = false; + infoGlobal.m_timeStep = 1.f/60.f; + infoGlobal.m_numIterations = 4;//4; +// infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION; + //infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS; + infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS; + + //if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) + //if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) + + + solveGroup(bodies,inertias,numBodies,contacts,numContacts,constraints,numConstraints,infoGlobal); + + if (!numContacts) + return; +} + + + + +/// b3PgsJacobiSolver Sequentially applies impulses +b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyData* bodies, + b3InertiaData* inertias, + int numBodies, + b3Contact4* manifoldPtr, + int numManifolds, + b3TypedConstraint** constraints, + int numConstraints, + const b3ContactSolverInfo& infoGlobal) +{ + + B3_PROFILE("solveGroup"); + //you need to provide at least some bodies + + solveGroupCacheFriendlySetup( bodies, inertias,numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal); + + solveGroupCacheFriendlyIterations(constraints, numConstraints,infoGlobal); + + solveGroupCacheFriendlyFinish(bodies, inertias,numBodies, infoGlobal); + + return 0.f; +} + + + + + + + + + +#ifdef USE_SIMD +#include +#define b3VecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) +static inline __m128 b3SimdDot3( __m128 vec0, __m128 vec1 ) +{ + __m128 result = _mm_mul_ps( vec0, vec1); + return _mm_add_ps( b3VecSplat( result, 0 ), _mm_add_ps( b3VecSplat( result, 1 ), b3VecSplat( result, 2 ) ) ); +} +#endif//USE_SIMD + +// Project Gauss Seidel or the equivalent Sequential Impulse +void b3PgsJacobiSolver::resolveSingleConstraintRowGenericSIMD(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c) +{ +#ifdef USE_SIMD + __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); + __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); + b3SimdScalar resultLowerLess,resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); + c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); + __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); + c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); + __m128 impulseMagnitude = deltaImpulse; + body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); +#else + resolveSingleConstraintRowGeneric(body1,body2,c); +#endif +} + +// Project Gauss Seidel or the equivalent Sequential Impulse + void b3PgsJacobiSolver::resolveSingleConstraintRowGeneric(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c) +{ + b3Scalar deltaImpulse = c.m_rhs-b3Scalar(c.m_appliedImpulse)*c.m_cfm; + const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + +// const b3Scalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + + const b3Scalar sum = b3Scalar(c.m_appliedImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else if (sum > c.m_upperLimit) + { + deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_upperLimit; + } + else + { + c.m_appliedImpulse = sum; + } + + body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); +} + + void b3PgsJacobiSolver::resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c) +{ +#ifdef USE_SIMD + __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); + __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); + b3SimdScalar resultLowerLess,resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); + c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); + __m128 impulseMagnitude = deltaImpulse; + body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); +#else + resolveSingleConstraintRowLowerLimit(body1,body2,c); +#endif +} + +// Project Gauss Seidel or the equivalent Sequential Impulse + void b3PgsJacobiSolver::resolveSingleConstraintRowLowerLimit(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c) +{ + b3Scalar deltaImpulse = c.m_rhs-b3Scalar(c.m_appliedImpulse)*c.m_cfm; + const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + const b3Scalar sum = b3Scalar(c.m_appliedImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else + { + c.m_appliedImpulse = sum; + } + body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); +} + + +void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly( + b3SolverBody& body1, + b3SolverBody& body2, + const b3SolverConstraint& c) +{ + if (c.m_rhsPenetration) + { + m_numSplitImpulseRecoveries++; + b3Scalar deltaImpulse = c.m_rhsPenetration-b3Scalar(c.m_appliedPushImpulse)*c.m_cfm; + const b3Scalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); + const b3Scalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); + + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + const b3Scalar sum = b3Scalar(c.m_appliedPushImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse; + c.m_appliedPushImpulse = c.m_lowerLimit; + } + else + { + c.m_appliedPushImpulse = sum; + } + body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + } +} + + void b3PgsJacobiSolver::resolveSplitPenetrationSIMD(b3SolverBody& body1,b3SolverBody& body2,const b3SolverConstraint& c) +{ +#ifdef USE_SIMD + if (!c.m_rhsPenetration) + return; + + m_numSplitImpulseRecoveries++; + + __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); + __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); + b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); + b3SimdScalar resultLowerLess,resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); + deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); + c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); + __m128 impulseMagnitude = deltaImpulse; + body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); +#else + resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); +#endif +} + + + +unsigned long b3PgsJacobiSolver::b3Rand2() +{ + m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; + return m_btSeed2; +} + + + +//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) +int b3PgsJacobiSolver::b3RandInt2 (int n) +{ + // seems good; xor-fold and modulus + const unsigned long un = static_cast(n); + unsigned long r = b3Rand2(); + + // note: probably more aggressive than it needs to be -- might be + // able to get away without one or two of the innermost branches. + if (un <= 0x00010000UL) { + r ^= (r >> 16); + if (un <= 0x00000100UL) { + r ^= (r >> 8); + if (un <= 0x00000010UL) { + r ^= (r >> 4); + if (un <= 0x00000004UL) { + r ^= (r >> 2); + if (un <= 0x00000002UL) { + r ^= (r >> 1); + } + } + } + } + } + + return (int) (r % un); +} + + + +void b3PgsJacobiSolver::initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* rb) +{ + + solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); + solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f); + solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + + if (rb) + { + solverBody->m_worldTransform = getWorldTransform(rb); + solverBody->internalSetInvMass(b3MakeVector3(rb->m_invMass,rb->m_invMass,rb->m_invMass)); + solverBody->m_originalBodyIndex = bodyIndex; + solverBody->m_angularFactor = b3MakeVector3(1,1,1); + solverBody->m_linearFactor = b3MakeVector3(1,1,1); + solverBody->m_linearVelocity = getLinearVelocity(rb); + solverBody->m_angularVelocity = getAngularVelocity(rb); + } else + { + solverBody->m_worldTransform.setIdentity(); + solverBody->internalSetInvMass(b3MakeVector3(0,0,0)); + solverBody->m_originalBodyIndex = bodyIndex; + solverBody->m_angularFactor.setValue(1,1,1); + solverBody->m_linearFactor.setValue(1,1,1); + solverBody->m_linearVelocity.setValue(0,0,0); + solverBody->m_angularVelocity.setValue(0,0,0); + } + + +} + + + + + + +b3Scalar b3PgsJacobiSolver::restitutionCurve(b3Scalar rel_vel, b3Scalar restitution) +{ + b3Scalar rest = restitution * -rel_vel; + return rest; +} + + + + + + +void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) +{ + + + solverConstraint.m_contactNormal = normalAxis; + b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; + b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; + + b3RigidBodyData* body0 = &bodies[solverBodyA.m_originalBodyIndex]; + b3RigidBodyData* body1 = &bodies[solverBodyB.m_originalBodyIndex]; + + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + + solverConstraint.m_friction = cp.m_combinedFriction; + solverConstraint.m_originalContactPoint = 0; + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + b3Vector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex])*ftorqueAxis1 : b3MakeVector3(0,0,0); + } + { + b3Vector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal); + solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex])*ftorqueAxis1 : b3MakeVector3(0,0,0); + } + + b3Scalar scaledDenom; + + { + b3Vector3 vec; + b3Scalar denom0 = 0.f; + b3Scalar denom1 = 0.f; + if (body0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = body0->m_invMass + normalAxis.dot(vec); + } + if (body1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = body1->m_invMass + normalAxis.dot(vec); + } + + b3Scalar denom; + if (m_usePgs) + { + scaledDenom = denom = relaxation/(denom0+denom1); + } else + { + denom = relaxation/(denom0+denom1); + b3Scalar countA = body0->m_invMass ? b3Scalar(m_bodyCount[solverBodyA.m_originalBodyIndex]): 1.f; + b3Scalar countB = body1->m_invMass ? b3Scalar(m_bodyCount[solverBodyB.m_originalBodyIndex]): 1.f; + + scaledDenom = relaxation/(denom0*countA+denom1*countB); + } + + solverConstraint.m_jacDiagABInv = denom; + } + + { + + + b3Scalar rel_vel; + b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:b3MakeVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:b3MakeVector3(0,0,0)); + b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:b3MakeVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:b3MakeVector3(0,0,0)); + + rel_vel = vel1Dotn+vel2Dotn; + +// b3Scalar positionalError = 0.f; + + b3SimdScalar velocityError = desiredVelocity - rel_vel; + b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(scaledDenom);//solverConstraint.m_jacDiagABInv); + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_cfm = cfmSlip; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + + } +} + +b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) +{ + b3SolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); + solverConstraint.m_frictionIndex = frictionIndex; + setupFrictionConstraint(bodies,inertias,solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); + return solverConstraint; +} + + +void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, + b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, + b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, + b3Scalar desiredVelocity, b3Scalar cfmSlip) + +{ + b3Vector3 normalAxis=b3MakeVector3(0,0,0); + + + solverConstraint.m_contactNormal = normalAxis; + b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; + b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; + + b3RigidBodyData* body0 = &bodies[m_tmpSolverBodyPool[solverBodyIdA].m_originalBodyIndex]; + b3RigidBodyData* body1 = &bodies[m_tmpSolverBodyPool[solverBodyIdB].m_originalBodyIndex]; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + + solverConstraint.m_friction = cp.m_combinedRollingFriction; + solverConstraint.m_originalContactPoint = 0; + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + b3Vector3 ftorqueAxis1 = -normalAxis1; + solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex])*ftorqueAxis1 : b3MakeVector3(0,0,0); + } + { + b3Vector3 ftorqueAxis1 = normalAxis1; + solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex])*ftorqueAxis1 : b3MakeVector3(0,0,0); + } + + + { + b3Vector3 iMJaA = body0?getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex])*solverConstraint.m_relpos1CrossNormal:b3MakeVector3(0,0,0); + b3Vector3 iMJaB = body1?getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex])*solverConstraint.m_relpos2CrossNormal:b3MakeVector3(0,0,0); + b3Scalar sum = 0; + sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); + solverConstraint.m_jacDiagABInv = b3Scalar(1.)/sum; + } + + { + + + b3Scalar rel_vel; + b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:b3MakeVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:b3MakeVector3(0,0,0)); + b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:b3MakeVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:b3MakeVector3(0,0,0)); + + rel_vel = vel1Dotn+vel2Dotn; + +// b3Scalar positionalError = 0.f; + + b3SimdScalar velocityError = desiredVelocity - rel_vel; + b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(solverConstraint.m_jacDiagABInv); + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_cfm = cfmSlip; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + + } +} + + + + + + + + +b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip) +{ + b3SolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); + solverConstraint.m_frictionIndex = frictionIndex; + setupRollingFrictionConstraint(bodies,inertias,solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); + return solverConstraint; +} + + +int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias) +{ + //b3Assert(bodyIndex< m_tmpSolverBodyPool.size()); + + b3RigidBodyData& body = bodies[bodyIndex]; + int curIndex = -1; + if (m_usePgs || body.m_invMass==0.f) + { + if (m_bodyCount[bodyIndex]<0) + { + curIndex = m_tmpSolverBodyPool.size(); + b3SolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(bodyIndex,&solverBody,&body); + solverBody.m_originalBodyIndex = bodyIndex; + m_bodyCount[bodyIndex] = curIndex; + } else + { + curIndex = m_bodyCount[bodyIndex]; + } + } else + { + b3Assert(m_bodyCount[bodyIndex]>0); + m_bodyCountCheck[bodyIndex]++; + curIndex = m_tmpSolverBodyPool.size(); + b3SolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(bodyIndex,&solverBody,&body); + solverBody.m_originalBodyIndex = bodyIndex; + } + + b3Assert(curIndex>=0); + return curIndex; + +} +#include + + +void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal, + b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, + b3Vector3& rel_pos1, b3Vector3& rel_pos2) +{ + + const b3Vector3& pos1 = cp.getPositionWorldOnA(); + const b3Vector3& pos2 = cp.getPositionWorldOnB(); + + b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + + b3RigidBodyData* rb0 = &bodies[bodyA->m_originalBodyIndex]; + b3RigidBodyData* rb1 = &bodies[bodyB->m_originalBodyIndex]; + +// b3Vector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); +// b3Vector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = 1.f; + + b3Vector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0 ? getInvInertiaTensorWorld(&inertias[bodyA->m_originalBodyIndex])*torqueAxis0 : b3MakeVector3(0,0,0); + b3Vector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1 ? getInvInertiaTensorWorld(&inertias[bodyB->m_originalBodyIndex])*-torqueAxis1 : b3MakeVector3(0,0,0); + + b3Scalar scaledDenom; + { +#ifdef COMPUTE_IMPULSE_DENOM + b3Scalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); + b3Scalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); +#else + b3Vector3 vec; + b3Scalar denom0 = 0.f; + b3Scalar denom1 = 0.f; + if (rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->m_invMass + cp.m_normalWorldOnB.dot(vec); + } + if (rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->m_invMass + cp.m_normalWorldOnB.dot(vec); + } +#endif //COMPUTE_IMPULSE_DENOM + + + b3Scalar denom; + if (m_usePgs) + { + scaledDenom = denom = relaxation/(denom0+denom1); + } else + { + denom = relaxation/(denom0+denom1); + + b3Scalar countA = rb0->m_invMass? b3Scalar(m_bodyCount[bodyA->m_originalBodyIndex]) : 1.f; + b3Scalar countB = rb1->m_invMass? b3Scalar(m_bodyCount[bodyB->m_originalBodyIndex]) : 1.f; + scaledDenom = relaxation/(denom0*countA+denom1*countB); + } + solverConstraint.m_jacDiagABInv = denom; + } + + solverConstraint.m_contactNormal = cp.m_normalWorldOnB; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + + b3Scalar restitution = 0.f; + b3Scalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; + + { + b3Vector3 vel1,vel2; + + vel1 = rb0? getVelocityInLocalPoint(rb0,rel_pos1) : b3MakeVector3(0,0,0); + vel2 = rb1? getVelocityInLocalPoint(rb1, rel_pos2) : b3MakeVector3(0,0,0); + + // b3Vector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : b3Vector3(0,0,0); + vel = vel1 - vel2; + rel_vel = cp.m_normalWorldOnB.dot(vel); + + + + solverConstraint.m_friction = cp.m_combinedFriction; + + + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (restitution <= b3Scalar(0.)) + { + restitution = 0.f; + }; + } + + + ///warm starting (or zero if disabled) + if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal*bodyA->internalGetInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + if (rb1) + bodyB->internalApplyImpulse(solverConstraint.m_contactNormal*bodyB->internalGetInvMass(),-solverConstraint.m_angularComponentB,-(b3Scalar)solverConstraint.m_appliedImpulse); + } else + { + solverConstraint.m_appliedImpulse = 0.f; + } + + solverConstraint.m_appliedPushImpulse = 0.f; + + { + b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?bodyA->m_linearVelocity:b3MakeVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:b3MakeVector3(0,0,0)); + b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?bodyB->m_linearVelocity:b3MakeVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:b3MakeVector3(0,0,0)); + b3Scalar rel_vel = vel1Dotn+vel2Dotn; + + b3Scalar positionalError = 0.f; + b3Scalar velocityError = restitution - rel_vel;// * damping; + + + b3Scalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + + if (penetration>0) + { + positionalError = 0; + + velocityError -= penetration / infoGlobal.m_timeStep; + } else + { + positionalError = -penetration * erp/infoGlobal.m_timeStep; + } + + b3Scalar penetrationImpulse = positionalError*scaledDenom;//solverConstraint.m_jacDiagABInv; + b3Scalar velocityImpulse = velocityError *scaledDenom;//solverConstraint.m_jacDiagABInv; + + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } + + + + +} + + + +void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal) +{ + + b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + + + { + b3SolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; + if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) + { + frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; + if (bodies[bodyA->m_originalBodyIndex].m_invMass) + bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal*bodies[bodyA->m_originalBodyIndex].m_invMass,frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); + if (bodies[bodyB->m_originalBodyIndex].m_invMass) + bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal*bodies[bodyB->m_originalBodyIndex].m_invMass,-frictionConstraint1.m_angularComponentB,-(b3Scalar)frictionConstraint1.m_appliedImpulse); + } else + { + frictionConstraint1.m_appliedImpulse = 0.f; + } + } + + if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + b3SolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; + if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING) + { + frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; + if (bodies[bodyA->m_originalBodyIndex].m_invMass) + bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal*bodies[bodyA->m_originalBodyIndex].m_invMass,frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); + if (bodies[bodyB->m_originalBodyIndex].m_invMass) + bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal*bodies[bodyB->m_originalBodyIndex].m_invMass,-frictionConstraint2.m_angularComponentB,-(b3Scalar)frictionConstraint2.m_appliedImpulse); + } else + { + frictionConstraint2.m_appliedImpulse = 0.f; + } + } +} + + + + +void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal) +{ + b3RigidBodyData* colObj0=0,*colObj1=0; + + + int solverBodyIdA = getOrInitSolverBody(manifold->getBodyA(),bodies,inertias); + int solverBodyIdB = getOrInitSolverBody(manifold->getBodyB(),bodies,inertias); + +// b3RigidBody* bodyA = b3RigidBody::upcast(colObj0); +// b3RigidBody* bodyB = b3RigidBody::upcast(colObj1); + + b3SolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + b3SolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + + + + ///avoid collision response between two static objects + if (solverBodyA->m_invMass.isZero() && solverBodyB->m_invMass.isZero()) + return; + + int rollingFriction=1; + int numContacts = getNumContacts(manifold); + for (int j=0;jgetAngularVelocity(angVelA); + solverBodyB->getAngularVelocity(angVelB); + b3Vector3 relAngVel = angVelB-angVelA; + + if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) + { + //only a single rollingFriction per manifold + rollingFriction--; + if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) + { + relAngVel.normalize(); + if (relAngVel.length()>0.001) + addRollingFrictionConstraint(bodies,inertias,relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } else + { + addRollingFrictionConstraint(bodies,inertias,cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + b3Vector3 axis0,axis1; + b3PlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); + if (axis0.length()>0.001) + addRollingFrictionConstraint(bodies,inertias,axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if (axis1.length()>0.001) + addRollingFrictionConstraint(bodies,inertias,axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } + } + + ///Bullet has several options to set the friction directions + ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///based on the relative linear velocity. + ///If the relative velocity it zero, it will automatically compute a friction direction. + + ///You can also enable two friction directions, using the B3_SOLVER_USE_2_FRICTION_DIRECTIONS. + ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. + /// + ///If you choose B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. + /// + ///The user can manually override the friction directions for certain contacts using a contact callback, + ///and set the cp.m_lateralFrictionInitialized to true + ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) + ///this will give a conveyor belt effect + /// + if (!(infoGlobal.m_solverMode & B3_SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) + { + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + b3Scalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); + if (!(infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > B3_EPSILON) + { + cp.m_lateralFrictionDir1 *= 1.f/b3Sqrt(lat_rel_vel); + if((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); + cp.m_lateralFrictionDir2.normalize();//?? + addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } + + addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } else + { + b3PlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + + if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } + + addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) + { + cp.m_lateralFrictionInitialized = true; + } + } + + } else + { + addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); + + if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) + addFrictionConstraint(bodies,inertias,cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); + + setFrictionConstraintImpulse( bodies,inertias,solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); + } + + + + + } + } +} + +b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +{ + B3_PROFILE("solveGroupCacheFriendlySetup"); + + + m_maxOverrideNumSolverIterations = 0; + + + + m_tmpSolverBodyPool.resize(0); + + + m_bodyCount.resize(0); + m_bodyCount.resize(numBodies,0); + m_bodyCountCheck.resize(0); + m_bodyCountCheck.resize(numBodies,0); + + m_deltaLinearVelocities.resize(0); + m_deltaLinearVelocities.resize(numBodies,b3MakeVector3(0,0,0)); + m_deltaAngularVelocities.resize(0); + m_deltaAngularVelocities.resize(numBodies,b3MakeVector3(0,0,0)); + + //int totalBodies = 0; + + for (int i=0;igetRigidBodyA(); + int bodyIndexB = constraints[i]->getRigidBodyB(); + if (m_usePgs) + { + m_bodyCount[bodyIndexA]=-1; + m_bodyCount[bodyIndexB]=-1; + } else + { + //didn't implement joints with Jacobi version yet + b3Assert(0); + } + + } + for (int i=0;iinternalSetAppliedImpulse(0.0f); + } + } + + //b3RigidBody* rb0=0,*rb1=0; + //if (1) + { + { + + int totalNumRows = 0; + int i; + + m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); + //calculate the total number of contraint rows + for (i=0;igetJointFeedback(); + if (fb) + { + fb->m_appliedForceBodyA.setZero(); + fb->m_appliedTorqueBodyA.setZero(); + fb->m_appliedForceBodyB.setZero(); + fb->m_appliedTorqueBodyB.setZero(); + } + + if (constraints[i]->isEnabled()) + { + } + if (constraints[i]->isEnabled()) + { + constraints[i]->getInfo1(&info1,bodies); + } else + { + info1.m_numConstraintRows = 0; + info1.nub = 0; + } + totalNumRows += info1.m_numConstraintRows; + } + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); + + +#ifndef DISABLE_JOINTS + ///setup the b3SolverConstraints + int currentRow = 0; + + for (i=0;igetRigidBodyA()]; + //b3RigidBody& rbA = constraint->getRigidBodyA(); + // b3RigidBody& rbB = constraint->getRigidBodyB(); + b3RigidBodyData& rbB = bodies[ constraint->getRigidBodyB()]; + + int solverBodyIdA = getOrInitSolverBody(constraint->getRigidBodyA(),bodies,inertias); + int solverBodyIdB = getOrInitSolverBody(constraint->getRigidBodyB(),bodies,inertias); + + b3SolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; + b3SolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; + + + + + int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; + if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) + m_maxOverrideNumSolverIterations = overrideNumSolverIterations; + + + int j; + for ( j=0;jinternalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + + + b3TypedConstraint::b3ConstraintInfo2 info2; + info2.fps = 1.f/infoGlobal.m_timeStep; + info2.erp = infoGlobal.m_erp; + info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; + info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; + info2.m_J2linearAxis = 0; + info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; + info2.rowskip = sizeof(b3SolverConstraint)/sizeof(b3Scalar);//check this + ///the size of b3SolverConstraint needs be a multiple of b3Scalar + b3Assert(info2.rowskip*sizeof(b3Scalar)== sizeof(b3SolverConstraint)); + info2.m_constraintError = ¤tConstraintRow->m_rhs; + currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; + info2.m_damping = infoGlobal.m_damping; + info2.cfm = ¤tConstraintRow->m_cfm; + info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; + info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + info2.m_numIterations = infoGlobal.m_numIterations; + constraints[i]->getInfo2(&info2,bodies); + + ///finalize the constraint setup + for ( j=0;j=constraints[i]->getBreakingImpulseThreshold()) + { + solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold(); + } + + if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold()) + { + solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold(); + } + + solverConstraint.m_originalContactPoint = constraint; + + b3Matrix3x3& invInertiaWorldA= inertias[constraint->getRigidBodyA()].m_invInertiaWorld; + { + + //b3Vector3 angularFactorA(1,1,1); + const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; + solverConstraint.m_angularComponentA = invInertiaWorldA*ftorqueAxis1;//*angularFactorA; + } + + b3Matrix3x3& invInertiaWorldB= inertias[constraint->getRigidBodyB()].m_invInertiaWorld; + { + + const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; + solverConstraint.m_angularComponentB = invInertiaWorldB*ftorqueAxis2;//*constraint->getRigidBodyB().getAngularFactor(); + } + + { + //it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal + //because it gets multiplied iMJlB + b3Vector3 iMJlA = solverConstraint.m_contactNormal*rbA.m_invMass; + b3Vector3 iMJaA = invInertiaWorldA*solverConstraint.m_relpos1CrossNormal; + b3Vector3 iMJlB = solverConstraint.m_contactNormal*rbB.m_invMass;//sign of normal? + b3Vector3 iMJaB = invInertiaWorldB*solverConstraint.m_relpos2CrossNormal; + + b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal); + sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + sum += iMJlB.dot(solverConstraint.m_contactNormal); + sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); + b3Scalar fsum = b3Fabs(sum); + b3Assert(fsum > B3_EPSILON); + solverConstraint.m_jacDiagABInv = fsum>B3_EPSILON?b3Scalar(1.)/sum : 0.f; + } + + + ///fix rhs + ///todo: add force/torque accelerators + { + b3Scalar rel_vel; + b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.m_linVel) + solverConstraint.m_relpos1CrossNormal.dot(rbA.m_angVel); + b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.m_linVel) + solverConstraint.m_relpos2CrossNormal.dot(rbB.m_angVel); + + rel_vel = vel1Dotn+vel2Dotn; + + b3Scalar restitution = 0.f; + b3Scalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 + b3Scalar velocityError = restitution - rel_vel * info2.m_damping; + b3Scalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + b3Scalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_appliedImpulse = 0.f; + + } + } + } + currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; + } +#endif //DISABLE_JOINTS + } + + + { + int i; + + for (i=0;ib3Scalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + } + + if (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) + { + + b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; + + if (totalImpulse>b3Scalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + } + } + } + + } + else//B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS + { + //solve the friction constraints after all contact constraints, don't interleave them + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + int j; + + for (j=0;jb3Scalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + } + + + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + for (j=0;jb3Scalar(0)) + { + b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); + } + } + + + } + } + } else + { + //non-SIMD version + ///solve all joint constraints + for (int j=0;jb3Scalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + } + + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + for (int j=0;jb3Scalar(0)) + { + b3Scalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); + } + } + } + } + return 0.f; +} + + +void b3PgsJacobiSolver::solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +{ + int iteration; + if (infoGlobal.m_splitImpulse) + { + if (infoGlobal.m_solverMode & B3_SOLVER_SIMD) + { + for ( iteration = 0;iteration infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + + for ( int iteration = 0 ; iteration< maxIterations ; iteration++) + //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) + { + + solveSingleIteration(iteration, constraints,numConstraints,infoGlobal); + + + if (!m_usePgs) + { + averageVelocities(); + } + } + + } + return 0.f; +} + +void b3PgsJacobiSolver::averageVelocities() +{ + B3_PROFILE("averaging"); + //average the velocities + int numBodies = m_bodyCount.size(); + + m_deltaLinearVelocities.resize(0); + m_deltaLinearVelocities.resize(numBodies,b3MakeVector3(0,0,0)); + m_deltaAngularVelocities.resize(0); + m_deltaAngularVelocities.resize(numBodies,b3MakeVector3(0,0,0)); + + for (int i=0;im_appliedImpulse = solveManifold.m_appliedImpulse; + // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + // printf("pt->m_appliedImpulseLateral1 = %f\n", f); + pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); + if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; + } + //do a callback here? + } + } + + numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); + for (j=0;jgetJointFeedback(); + if (fb) + { + b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverConstr.m_solverBodyIdA]; + b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverConstr.m_solverBodyIdB]; + + fb->m_appliedForceBodyA += solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*bodyA->m_linearFactor/infoGlobal.m_timeStep; + fb->m_appliedForceBodyB += -solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*bodyB->m_linearFactor/infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* bodyA->m_angularFactor*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyB += -solverConstr.m_relpos1CrossNormal* bodyB->m_angularFactor*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; + + } + + constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); + if (b3Fabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) + { + constr->setEnabled(false); + } + } + + { + B3_PROFILE("write back velocities and transforms"); + for ( i=0;im_invMass) + { + if (infoGlobal.m_splitImpulse) + m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); + else + m_tmpSolverBodyPool[i].writebackVelocity(); + + if (m_usePgs) + { + body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity; + body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity; + } else + { + b3Scalar factor = 1.f/b3Scalar(m_bodyCount[bodyIndex]); + + b3Vector3 deltaLinVel = m_deltaLinearVelocities[bodyIndex]*factor; + b3Vector3 deltaAngVel = m_deltaAngularVelocities[bodyIndex]*factor; + //printf("body %d\n",bodyIndex); + //printf("deltaLinVel = %f,%f,%f\n",deltaLinVel.getX(),deltaLinVel.getY(),deltaLinVel.getZ()); + //printf("deltaAngVel = %f,%f,%f\n",deltaAngVel.getX(),deltaAngVel.getY(),deltaAngVel.getZ()); + + body->m_linVel += deltaLinVel; + body->m_angVel += deltaAngVel; + } + + if (infoGlobal.m_splitImpulse) + { + body->m_pos = m_tmpSolverBodyPool[i].m_worldTransform.getOrigin(); + b3Quaternion orn; + orn = m_tmpSolverBodyPool[i].m_worldTransform.getRotation(); + body->m_quat = orn; + } + } + } + } + + + m_tmpSolverContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); + + m_tmpSolverBodyPool.resizeNoInitialize(0); + return 0.f; +} + + + +void b3PgsJacobiSolver::reset() +{ + m_btSeed2 = 0; +} \ No newline at end of file diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h new file mode 100644 index 000000000000..d2ca307fab38 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h @@ -0,0 +1,149 @@ +#ifndef B3_PGS_JACOBI_SOLVER +#define B3_PGS_JACOBI_SOLVER + + +struct b3Contact4; +struct b3ContactPoint; + + +class b3Dispatcher; + +#include "b3TypedConstraint.h" +#include "b3ContactSolverInfo.h" +#include "b3SolverBody.h" +#include "b3SolverConstraint.h" + +struct b3RigidBodyData; +struct b3InertiaData; + +class b3PgsJacobiSolver +{ + +protected: + b3AlignedObjectArray m_tmpSolverBodyPool; + b3ConstraintArray m_tmpSolverContactConstraintPool; + b3ConstraintArray m_tmpSolverNonContactConstraintPool; + b3ConstraintArray m_tmpSolverContactFrictionConstraintPool; + b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; + + b3AlignedObjectArray m_orderTmpConstraintPool; + b3AlignedObjectArray m_orderNonContactConstraintPool; + b3AlignedObjectArray m_orderFrictionConstraintPool; + b3AlignedObjectArray m_tmpConstraintSizesPool; + + b3AlignedObjectArray m_bodyCount; + b3AlignedObjectArray m_bodyCountCheck; + + b3AlignedObjectArray m_deltaLinearVelocities; + b3AlignedObjectArray m_deltaAngularVelocities; + + bool m_usePgs; + void averageVelocities(); + + int m_maxOverrideNumSolverIterations; + + int m_numSplitImpulseRecoveries; + + b3Scalar getContactProcessingThreshold(b3Contact4* contact) + { + return 0.02f; + } + void setupFrictionConstraint( b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB, + b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, + b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, + b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.); + + void setupRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB, + b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2, + b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, + b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.); + + b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.); + b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies,b3InertiaData* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyData* colObj0,b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f); + + + void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, + b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, + const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, + b3Vector3& rel_pos1, b3Vector3& rel_pos2); + + void setFrictionConstraintImpulse( b3RigidBodyData* bodies, b3InertiaData* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, + b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal); + + ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction + unsigned long m_btSeed2; + + + b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution); + + void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal); + + + void resolveSplitPenetrationSIMD( + b3SolverBody& bodyA,b3SolverBody& bodyB, + const b3SolverConstraint& contactConstraint); + + void resolveSplitPenetrationImpulseCacheFriendly( + b3SolverBody& bodyA,b3SolverBody& bodyB, + const b3SolverConstraint& contactConstraint); + + //internal method + int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias); + void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* collisionObject); + + void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint); + + void resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint); + + void resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint); + + void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint); + +protected: + + virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + + + virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + + + virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal); + + +public: + + B3_DECLARE_ALIGNED_ALLOCATOR(); + + b3PgsJacobiSolver(bool usePgs); + virtual ~b3PgsJacobiSolver(); + +// void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts); + void solveContacts(int numBodies, b3RigidBodyData* bodies, b3InertiaData* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints); + + b3Scalar solveGroup(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + + ///clear internal cached data and reset random seed + virtual void reset(); + + unsigned long b3Rand2(); + + int b3RandInt2 (int n); + + void setRandSeed(unsigned long seed) + { + m_btSeed2 = seed; + } + unsigned long getRandSeed() const + { + return m_btSeed2; + } + + + + +}; + +#endif //B3_PGS_JACOBI_SOLVER + diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp new file mode 100644 index 000000000000..02c11db32097 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp @@ -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 "b3Point2PointConstraint.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + +#include + + + + + +b3Point2PointConstraint::b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB) +:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), +m_flags(0) +{ + +} + +/* +b3Point2PointConstraint::b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA) +:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), +m_flags(0), +m_useSolveConstraintObsolete(false) +{ + +} +*/ + + +void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +{ + getInfo1NonVirtual(info,bodies); +} + +void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies) +{ + info->m_numConstraintRows = 3; + info->nub = 3; +} + + + + +void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies) +{ + b3Transform trA; + trA.setIdentity(); + trA.setOrigin(bodies[m_rbA].m_pos); + trA.setRotation(bodies[m_rbA].m_quat); + + b3Transform trB; + trB.setIdentity(); + trB.setOrigin(bodies[m_rbB].m_pos); + trB.setRotation(bodies[m_rbB].m_quat); + + getInfo2NonVirtual(info, trA,trB); +} + +void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans) +{ + + //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; + + b3Vector3 a1 = body0_trans.getBasis()*getPivotInA(); + //b3Vector3 a1a = b3QuatRotate(body0_trans.getRotation(),getPivotInA()); + + { + b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis); + b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip); + b3Vector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + if (info->m_J2linearAxis) + { + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip+1] = -1; + info->m_J2linearAxis[2*info->rowskip+2] = -1; + } + + b3Vector3 a2 = body1_trans.getBasis()*getPivotInB(); + + { + // b3Vector3 a2n = -a2; + b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis); + b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + + + // set right hand side + b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp; + b3Scalar k = info->fps * currERP; + 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]); + } + if(m_flags & B3_P2P_FLAGS_CFM) + { + for (j=0; j<3; j++) + { + info->cfm[j*info->rowskip] = m_cfm; + } + } + + b3Scalar 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; + } + } + info->m_damping = m_setting.m_damping; + +} + + + +void b3Point2PointConstraint::updateRHS(b3Scalar timeStep) +{ + (void)timeStep; + +} + +///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +///If no axis is provided, it uses the default axis for this constraint. +void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis) +{ + if(axis != -1) + { + b3AssertConstrParams(0); + } + else + { + switch(num) + { + case B3_CONSTRAINT_ERP : + case B3_CONSTRAINT_STOP_ERP : + m_erp = value; + m_flags |= B3_P2P_FLAGS_ERP; + break; + case B3_CONSTRAINT_CFM : + case B3_CONSTRAINT_STOP_CFM : + m_cfm = value; + m_flags |= B3_P2P_FLAGS_CFM; + break; + default: + b3AssertConstrParams(0); + } + } +} + +///return the local value of parameter +b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const +{ + b3Scalar retVal(B3_INFINITY); + if(axis != -1) + { + b3AssertConstrParams(0); + } + else + { + switch(num) + { + case B3_CONSTRAINT_ERP : + case B3_CONSTRAINT_STOP_ERP : + b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP); + retVal = m_erp; + break; + case B3_CONSTRAINT_CFM : + case B3_CONSTRAINT_STOP_CFM : + b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM); + retVal = m_cfm; + break; + default: + b3AssertConstrParams(0); + } + } + return retVal; +} + diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h new file mode 100644 index 000000000000..681b487334c7 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h @@ -0,0 +1,159 @@ +/* +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 B3_POINT2POINTCONSTRAINT_H +#define B3_POINT2POINTCONSTRAINT_H + +#include "Bullet3Common/b3Vector3.h" +//#include "b3JacobianEntry.h" +#include "b3TypedConstraint.h" + +class b3RigidBody; + + +#ifdef B3_USE_DOUBLE_PRECISION +#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData +#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData" +#else +#define b3Point2PointConstraintData b3Point2PointConstraintFloatData +#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData" +#endif //B3_USE_DOUBLE_PRECISION + +struct b3ConstraintSetting +{ + b3ConstraintSetting() : + m_tau(b3Scalar(0.3)), + m_damping(b3Scalar(1.)), + m_impulseClamp(b3Scalar(0.)) + { + } + b3Scalar m_tau; + b3Scalar m_damping; + b3Scalar m_impulseClamp; +}; + +enum b3Point2PointFlags +{ + B3_P2P_FLAGS_ERP = 1, + B3_P2P_FLAGS_CFM = 2 +}; + +/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space +B3_ATTRIBUTE_ALIGNED16(class) b3Point2PointConstraint : public b3TypedConstraint +{ +#ifdef IN_PARALLELL_SOLVER +public: +#endif + + b3Vector3 m_pivotInA; + b3Vector3 m_pivotInB; + + int m_flags; + b3Scalar m_erp; + b3Scalar m_cfm; + +public: + + B3_DECLARE_ALIGNED_ALLOCATOR(); + + b3ConstraintSetting m_setting; + + b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB); + + //b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA); + + + + virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + + void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyData* bodies); + + virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies); + + void getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans); + + void updateRHS(b3Scalar timeStep); + + void setPivotA(const b3Vector3& pivotA) + { + m_pivotInA = pivotA; + } + + void setPivotB(const b3Vector3& pivotB) + { + m_pivotInB = pivotB; + } + + const b3Vector3& getPivotInA() const + { + return m_pivotInA; + } + + const b3Vector3& getPivotInB() const + { + return m_pivotInB; + } + + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///If no axis is provided, it uses the default axis for this constraint. + virtual void setParam(int num, b3Scalar value, int axis = -1); + ///return the local value of parameter + virtual b3Scalar getParam(int num, int axis = -1) const; + +// virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +// virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const; + + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct b3Point2PointConstraintFloatData +{ + b3TypedConstraintData m_typeConstraintData; + b3Vector3FloatData m_pivotInA; + b3Vector3FloatData m_pivotInB; +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct b3Point2PointConstraintDoubleData +{ + b3TypedConstraintData m_typeConstraintData; + b3Vector3DoubleData m_pivotInA; + b3Vector3DoubleData m_pivotInB; +}; + +/* +B3_FORCE_INLINE int b3Point2PointConstraint::calculateSerializeBufferSize() const +{ + return sizeof(b3Point2PointConstraintData); + +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +B3_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer, b3Serializer* serializer) const +{ + b3Point2PointConstraintData* p2pData = (b3Point2PointConstraintData*)dataBuffer; + + b3TypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer); + m_pivotInA.serialize(p2pData->m_pivotInA); + m_pivotInB.serialize(p2pData->m_pivotInB); + + return b3Point2PointConstraintDataName; +} +*/ + +#endif //B3_POINT2POINTCONSTRAINT_H diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h new file mode 100644 index 000000000000..0049317d981b --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h @@ -0,0 +1,302 @@ +/* +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 B3_SOLVER_BODY_H +#define B3_SOLVER_BODY_H + + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Matrix3x3.h" + +#include "Bullet3Common/b3AlignedAllocator.h" +#include "Bullet3Common/b3TransformUtil.h" + +///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision +#ifdef B3_USE_SSE +#define USE_SIMD 1 +#endif // + + +#ifdef USE_SIMD + +struct b3SimdScalar +{ + B3_FORCE_INLINE b3SimdScalar() + { + + } + + B3_FORCE_INLINE b3SimdScalar(float fl) + :m_vec128 (_mm_set1_ps(fl)) + { + } + + B3_FORCE_INLINE b3SimdScalar(__m128 v128) + :m_vec128(v128) + { + } + union + { + __m128 m_vec128; + float m_floats[4]; + float x,y,z,w; + int m_ints[4]; + b3Scalar m_unusedPadding; + }; + B3_FORCE_INLINE __m128 get128() + { + return m_vec128; + } + + B3_FORCE_INLINE const __m128 get128() const + { + return m_vec128; + } + + B3_FORCE_INLINE void set128(__m128 v128) + { + m_vec128 = v128; + } + + B3_FORCE_INLINE operator __m128() + { + return m_vec128; + } + B3_FORCE_INLINE operator const __m128() const + { + return m_vec128; + } + + B3_FORCE_INLINE operator float() const + { + return m_floats[0]; + } + +}; + +///@brief Return the elementwise product of two b3SimdScalar +B3_FORCE_INLINE b3SimdScalar +operator*(const b3SimdScalar& v1, const b3SimdScalar& v2) +{ + return b3SimdScalar(_mm_mul_ps(v1.get128(),v2.get128())); +} + +///@brief Return the elementwise product of two b3SimdScalar +B3_FORCE_INLINE b3SimdScalar +operator+(const b3SimdScalar& v1, const b3SimdScalar& v2) +{ + return b3SimdScalar(_mm_add_ps(v1.get128(),v2.get128())); +} + + +#else +#define b3SimdScalar b3Scalar +#endif + +///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. +B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); + b3Transform m_worldTransform; + b3Vector3 m_deltaLinearVelocity; + b3Vector3 m_deltaAngularVelocity; + b3Vector3 m_angularFactor; + b3Vector3 m_linearFactor; + b3Vector3 m_invMass; + b3Vector3 m_pushVelocity; + b3Vector3 m_turnVelocity; + b3Vector3 m_linearVelocity; + b3Vector3 m_angularVelocity; + + union + { + void* m_originalBody; + int m_originalBodyIndex; + }; + + int padding[3]; + + + void setWorldTransform(const b3Transform& worldTransform) + { + m_worldTransform = worldTransform; + } + + const b3Transform& getWorldTransform() const + { + return m_worldTransform; + } + + B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const + { + if (m_originalBody) + velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + else + velocity.setValue(0,0,0); + } + + B3_FORCE_INLINE void getAngularVelocity(b3Vector3& angVel) const + { + if (m_originalBody) + angVel =m_angularVelocity+m_deltaAngularVelocity; + else + angVel.setValue(0,0,0); + } + + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude) + { + if (m_originalBody) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,b3Scalar impulseMagnitude) + { + if (m_originalBody) + { + m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + + + const b3Vector3& getDeltaLinearVelocity() const + { + return m_deltaLinearVelocity; + } + + const b3Vector3& getDeltaAngularVelocity() const + { + return m_deltaAngularVelocity; + } + + const b3Vector3& getPushVelocity() const + { + return m_pushVelocity; + } + + const b3Vector3& getTurnVelocity() const + { + return m_turnVelocity; + } + + + //////////////////////////////////////////////// + ///some internal methods, don't use them + + b3Vector3& internalGetDeltaLinearVelocity() + { + return m_deltaLinearVelocity; + } + + b3Vector3& internalGetDeltaAngularVelocity() + { + return m_deltaAngularVelocity; + } + + const b3Vector3& internalGetAngularFactor() const + { + return m_angularFactor; + } + + const b3Vector3& internalGetInvMass() const + { + return m_invMass; + } + + void internalSetInvMass(const b3Vector3& invMass) + { + m_invMass = invMass; + } + + b3Vector3& internalGetPushVelocity() + { + return m_pushVelocity; + } + + b3Vector3& internalGetTurnVelocity() + { + return m_turnVelocity; + } + + B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const + { + velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + } + + B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3& angVel) const + { + angVel = m_angularVelocity+m_deltaAngularVelocity; + } + + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude) + { + //if (m_originalBody) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + + + + void writebackVelocity() + { + //if (m_originalBody>=0) + { + m_linearVelocity +=m_deltaLinearVelocity; + m_angularVelocity += m_deltaAngularVelocity; + + //m_originalBody->setCompanionId(-1); + } + } + + + void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp) + { + (void) timeStep; + if (m_originalBody) + { + m_linearVelocity += m_deltaLinearVelocity; + m_angularVelocity += m_deltaAngularVelocity; + + //correct the position/orientation based on push/turn recovery + b3Transform newTransform; + if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0) + { + // b3Quaternion orn = m_worldTransform.getRotation(); + b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform); + m_worldTransform = newTransform; + } + //m_worldTransform.setRotation(orn); + //m_originalBody->setCompanionId(-1); + } + } + + + +}; + +#endif //B3_SOLVER_BODY_H + + diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h new file mode 100644 index 000000000000..bce83d460898 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h @@ -0,0 +1,80 @@ +/* +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 B3_SOLVER_CONSTRAINT_H +#define B3_SOLVER_CONSTRAINT_H + + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Matrix3x3.h" +//#include "b3JacobianEntry.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +//#define NO_FRICTION_TANGENTIALS 1 +#include "b3SolverBody.h" + + +///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. +B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); + + b3Vector3 m_relpos1CrossNormal; + b3Vector3 m_contactNormal; + + b3Vector3 m_relpos2CrossNormal; + //b3Vector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal + + b3Vector3 m_angularComponentA; + b3Vector3 m_angularComponentB; + + mutable b3SimdScalar m_appliedPushImpulse; + mutable b3SimdScalar m_appliedImpulse; + int m_padding1; + int m_padding2; + b3Scalar m_friction; + b3Scalar m_jacDiagABInv; + b3Scalar m_rhs; + b3Scalar m_cfm; + + b3Scalar m_lowerLimit; + b3Scalar m_upperLimit; + b3Scalar m_rhsPenetration; + union + { + void* m_originalContactPoint; + b3Scalar m_unusedPadding4; + }; + + int m_overrideNumSolverIterations; + int m_frictionIndex; + int m_solverBodyIdA; + int m_solverBodyIdB; + + + enum b3SolverConstraintType + { + B3_SOLVER_CONTACT_1D = 0, + B3_SOLVER_FRICTION_1D + }; +}; + +typedef b3AlignedObjectArray b3ConstraintArray; + + +#endif //B3_SOLVER_CONSTRAINT_H + + + diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp new file mode 100644 index 000000000000..699c481d64a6 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp @@ -0,0 +1,161 @@ +/* +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 "b3TypedConstraint.h" +//#include "Bullet3Common/b3Serializer.h" + + +#define B3_DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f) + + + +b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA,int rbB) +:b3TypedObject(type), +m_userConstraintType(-1), +m_userConstraintPtr((void*)-1), +m_breakingImpulseThreshold(B3_INFINITY), +m_isEnabled(true), +m_needsFeedback(false), +m_overrideNumSolverIterations(-1), +m_rbA(rbA), +m_rbB(rbB), +m_appliedImpulse(b3Scalar(0.)), +m_dbgDrawSize(B3_DEFAULT_DEBUGDRAW_SIZE), +m_jointFeedback(0) +{ +} + + + + +b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact) +{ + if(lowLim > uppLim) + { + return b3Scalar(1.0f); + } + else if(lowLim == uppLim) + { + return b3Scalar(0.0f); + } + b3Scalar lim_fact = b3Scalar(1.0f); + b3Scalar delta_max = vel / timeFact; + if(delta_max < b3Scalar(0.0f)) + { + if((pos >= lowLim) && (pos < (lowLim - delta_max))) + { + lim_fact = (lowLim - pos) / delta_max; + } + else if(pos < lowLim) + { + lim_fact = b3Scalar(0.0f); + } + else + { + lim_fact = b3Scalar(1.0f); + } + } + else if(delta_max > b3Scalar(0.0f)) + { + if((pos <= uppLim) && (pos > (uppLim - delta_max))) + { + lim_fact = (uppLim - pos) / delta_max; + } + else if(pos > uppLim) + { + lim_fact = b3Scalar(0.0f); + } + else + { + lim_fact = b3Scalar(1.0f); + } + } + else + { + lim_fact = b3Scalar(0.0f); + } + return lim_fact; +} + + + +void b3AngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor) +{ + m_halfRange = (high - low) / 2.0f; + m_center = b3NormalizeAngle(low + m_halfRange); + m_softness = _softness; + m_biasFactor = _biasFactor; + m_relaxationFactor = _relaxationFactor; +} + +void b3AngularLimit::test(const b3Scalar angle) +{ + m_correction = 0.0f; + m_sign = 0.0f; + m_solveLimit = false; + + if (m_halfRange >= 0.0f) + { + b3Scalar deviation = b3NormalizeAngle(angle - m_center); + if (deviation < -m_halfRange) + { + m_solveLimit = true; + m_correction = - (deviation + m_halfRange); + m_sign = +1.0f; + } + else if (deviation > m_halfRange) + { + m_solveLimit = true; + m_correction = m_halfRange - deviation; + m_sign = -1.0f; + } + } +} + + +b3Scalar b3AngularLimit::getError() const +{ + return m_correction * m_sign; +} + +void b3AngularLimit::fit(b3Scalar& angle) const +{ + if (m_halfRange > 0.0f) + { + b3Scalar relativeAngle = b3NormalizeAngle(angle - m_center); + if (!b3Equal(relativeAngle, m_halfRange)) + { + if (relativeAngle > 0.0f) + { + angle = getHigh(); + } + else + { + angle = getLow(); + } + } + } +} + +b3Scalar b3AngularLimit::getLow() const +{ + return b3NormalizeAngle(m_center - m_halfRange); +} + +b3Scalar b3AngularLimit::getHigh() const +{ + return b3NormalizeAngle(m_center + m_halfRange); +} diff --git a/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h new file mode 100644 index 000000000000..cf9cec0d5edf --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h @@ -0,0 +1,483 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2010 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 B3_TYPED_CONSTRAINT_H +#define B3_TYPED_CONSTRAINT_H + + +#include "Bullet3Common/b3Scalar.h" +#include "b3SolverConstraint.h" + +class b3Serializer; + +//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility +enum b3TypedConstraintType +{ + B3_POINT2POINT_CONSTRAINT_TYPE=3, + B3_HINGE_CONSTRAINT_TYPE, + B3_CONETWIST_CONSTRAINT_TYPE, + B3_D6_CONSTRAINT_TYPE, + B3_SLIDER_CONSTRAINT_TYPE, + B3_CONTACT_CONSTRAINT_TYPE, + B3_D6_SPRING_CONSTRAINT_TYPE, + B3_GEAR_CONSTRAINT_TYPE, + B3_FIXED_CONSTRAINT_TYPE, + B3_MAX_CONSTRAINT_TYPE +}; + + +enum b3ConstraintParams +{ + B3_CONSTRAINT_ERP=1, + B3_CONSTRAINT_STOP_ERP, + B3_CONSTRAINT_CFM, + B3_CONSTRAINT_STOP_CFM +}; + +#if 1 + #define b3AssertConstrParams(_par) b3Assert(_par) +#else + #define b3AssertConstrParams(_par) +#endif + + +B3_ATTRIBUTE_ALIGNED16(struct) b3JointFeedback +{ + b3Vector3 m_appliedForceBodyA; + b3Vector3 m_appliedTorqueBodyA; + b3Vector3 m_appliedForceBodyB; + b3Vector3 m_appliedTorqueBodyB; +}; + + +struct b3RigidBodyData; + + +///TypedConstraint is the baseclass for Bullet constraints and vehicles +B3_ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public b3TypedObject +{ + int m_userConstraintType; + + union + { + int m_userConstraintId; + void* m_userConstraintPtr; + }; + + b3Scalar m_breakingImpulseThreshold; + bool m_isEnabled; + bool m_needsFeedback; + int m_overrideNumSolverIterations; + + + b3TypedConstraint& operator=(b3TypedConstraint& other) + { + b3Assert(0); + (void) other; + return *this; + } + +protected: + int m_rbA; + int m_rbB; + b3Scalar m_appliedImpulse; + b3Scalar m_dbgDrawSize; + b3JointFeedback* m_jointFeedback; + + ///internal method used by the constraint solver, don't use them directly + b3Scalar getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact); + + +public: + + B3_DECLARE_ALIGNED_ALLOCATOR(); + + virtual ~b3TypedConstraint() {}; + b3TypedConstraint(b3TypedConstraintType type, int bodyA,int bodyB); + + struct b3ConstraintInfo1 { + int m_numConstraintRows,nub; + }; + + + + struct b3ConstraintInfo2 { + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + b3Scalar fps,erp; + + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + b3Scalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis; + + // elements to jump from one row to the next in J's + int rowskip; + + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + b3Scalar *m_constraintError,*cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + b3Scalar *m_lowerLimit,*m_upperLimit; + + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + int *findex; + // number of solver iterations + int m_numIterations; + + //damping of the velocity + b3Scalar m_damping; + }; + + int getOverrideNumSolverIterations() const + { + return m_overrideNumSolverIterations; + } + + ///override the number of constraint solver iterations used to solve this constraint + ///-1 will use the default number of iterations, as specified in SolverInfo.m_numIterations + void setOverrideNumSolverIterations(int overideNumIterations) + { + m_overrideNumSolverIterations = overideNumIterations; + } + + + ///internal method used by the constraint solver, don't use them directly + virtual void setupSolverConstraint(b3ConstraintArray& ca, int solverBodyA,int solverBodyB, b3Scalar timeStep) + { + (void)ca; + (void)solverBodyA; + (void)solverBodyB; + (void)timeStep; + } + + ///internal method used by the constraint solver, don't use them directly + virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyData* bodies)=0; + + ///internal method used by the constraint solver, don't use them directly + virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData* bodies)=0; + + ///internal method used by the constraint solver, don't use them directly + void internalSetAppliedImpulse(b3Scalar appliedImpulse) + { + m_appliedImpulse = appliedImpulse; + } + ///internal method used by the constraint solver, don't use them directly + b3Scalar internalGetAppliedImpulse() + { + return m_appliedImpulse; + } + + + b3Scalar getBreakingImpulseThreshold() const + { + return m_breakingImpulseThreshold; + } + + void setBreakingImpulseThreshold(b3Scalar threshold) + { + m_breakingImpulseThreshold = threshold; + } + + bool isEnabled() const + { + return m_isEnabled; + } + + void setEnabled(bool enabled) + { + m_isEnabled=enabled; + } + + + ///internal method used by the constraint solver, don't use them directly + virtual void solveConstraintObsolete(b3SolverBody& /*bodyA*/,b3SolverBody& /*bodyB*/,b3Scalar /*timeStep*/) {}; + + + int getRigidBodyA() const + { + return m_rbA; + } + int getRigidBodyB() const + { + return m_rbB; + } + + + int getRigidBodyA() + { + return m_rbA; + } + int getRigidBodyB() + { + return m_rbB; + } + + int getUserConstraintType() const + { + return m_userConstraintType ; + } + + void setUserConstraintType(int userConstraintType) + { + m_userConstraintType = userConstraintType; + }; + + void setUserConstraintId(int uid) + { + m_userConstraintId = uid; + } + + int getUserConstraintId() const + { + return m_userConstraintId; + } + + void setUserConstraintPtr(void* ptr) + { + m_userConstraintPtr = ptr; + } + + void* getUserConstraintPtr() + { + return m_userConstraintPtr; + } + + void setJointFeedback(b3JointFeedback* jointFeedback) + { + m_jointFeedback = jointFeedback; + } + + const b3JointFeedback* getJointFeedback() const + { + return m_jointFeedback; + } + + b3JointFeedback* getJointFeedback() + { + return m_jointFeedback; + } + + + int getUid() const + { + return m_userConstraintId; + } + + bool needsFeedback() const + { + return m_needsFeedback; + } + + ///enableFeedback will allow to read the applied linear and angular impulse + ///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information + void enableFeedback(bool needsFeedback) + { + m_needsFeedback = needsFeedback; + } + + ///getAppliedImpulse is an estimated total applied impulse. + ///This feedback could be used to determine breaking constraints or playing sounds. + b3Scalar getAppliedImpulse() const + { + b3Assert(m_needsFeedback); + return m_appliedImpulse; + } + + b3TypedConstraintType getConstraintType () const + { + return b3TypedConstraintType(m_objectType); + } + + void setDbgDrawSize(b3Scalar dbgDrawSize) + { + m_dbgDrawSize = dbgDrawSize; + } + b3Scalar getDbgDrawSize() + { + return m_dbgDrawSize; + } + + ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + ///If no axis is provided, it uses the default axis for this constraint. + virtual void setParam(int num, b3Scalar value, int axis = -1) = 0; + + ///return the local value of parameter + virtual b3Scalar getParam(int num, int axis = -1) const = 0; + +// virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + //virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const; + +}; + +// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits +// all arguments should be normalized angles (i.e. in range [-B3_PI, B3_PI]) +B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians) +{ + if(angleLowerLimitInRadians >= angleUpperLimitInRadians) + { + return angleInRadians; + } + else if(angleInRadians < angleLowerLimitInRadians) + { + b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleLowerLimitInRadians - angleInRadians)); + b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleUpperLimitInRadians - angleInRadians)); + return (diffLo < diffHi) ? angleInRadians : (angleInRadians + B3_2_PI); + } + else if(angleInRadians > angleUpperLimitInRadians) + { + b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleInRadians - angleUpperLimitInRadians)); + b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleInRadians - angleLowerLimitInRadians)); + return (diffLo < diffHi) ? (angleInRadians - B3_2_PI) : angleInRadians; + } + else + { + return angleInRadians; + } +} + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct b3TypedConstraintData +{ + int m_bodyA; + int m_bodyB; + char *m_name; + + int m_objectType; + int m_userConstraintType; + int m_userConstraintId; + int m_needsFeedback; + + float m_appliedImpulse; + float m_dbgDrawSize; + + int m_disableCollisionsBetweenLinkedBodies; + int m_overrideNumSolverIterations; + + float m_breakingImpulseThreshold; + int m_isEnabled; + +}; + +/*B3_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const +{ + return sizeof(b3TypedConstraintData); +} +*/ + + +class b3AngularLimit +{ +private: + b3Scalar + m_center, + m_halfRange, + m_softness, + m_biasFactor, + m_relaxationFactor, + m_correction, + m_sign; + + bool + m_solveLimit; + +public: + /// Default constructor initializes limit as inactive, allowing free constraint movement + b3AngularLimit() + :m_center(0.0f), + m_halfRange(-1.0f), + m_softness(0.9f), + m_biasFactor(0.3f), + m_relaxationFactor(1.0f), + m_correction(0.0f), + m_sign(0.0f), + m_solveLimit(false) + {} + + /// Sets all limit's parameters. + /// When low > high limit becomes inactive. + /// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit + void set(b3Scalar low, b3Scalar high, b3Scalar _softness = 0.9f, b3Scalar _biasFactor = 0.3f, b3Scalar _relaxationFactor = 1.0f); + + /// Checks conastaint angle against limit. If limit is active and the angle violates the limit + /// correction is calculated. + void test(const b3Scalar angle); + + /// Returns limit's softness + inline b3Scalar getSoftness() const + { + return m_softness; + } + + /// Returns limit's bias factor + inline b3Scalar getBiasFactor() const + { + return m_biasFactor; + } + + /// Returns limit's relaxation factor + inline b3Scalar getRelaxationFactor() const + { + return m_relaxationFactor; + } + + /// Returns correction value evaluated when test() was invoked + inline b3Scalar getCorrection() const + { + return m_correction; + } + + /// Returns sign value evaluated when test() was invoked + inline b3Scalar getSign() const + { + return m_sign; + } + + /// Gives half of the distance between min and max limit angle + inline b3Scalar getHalfRange() const + { + return m_halfRange; + } + + /// Returns true when the last test() invocation recognized limit violation + inline bool isLimit() const + { + return m_solveLimit; + } + + /// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle + /// returned is modified so it equals to the limit closest to given angle. + void fit(b3Scalar& angle) const; + + /// Returns correction value multiplied by sign value + b3Scalar getError() const; + + b3Scalar getLow() const; + + b3Scalar getHigh() const; + +}; + + + +#endif //B3_TYPED_CONSTRAINT_H diff --git a/extern/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp b/extern/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp new file mode 100644 index 000000000000..fbc84cc28d6e --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp @@ -0,0 +1,488 @@ +#include "b3CpuRigidBodyPipeline.h" + +#include "Bullet3Dynamics/shared/b3IntegrateTransforms.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Dynamics/shared/b3ContactConstraint4.h" +#include "Bullet3Dynamics/shared/b3Inertia.h" + + +struct b3CpuRigidBodyPipelineInternalData +{ + b3AlignedObjectArray m_rigidBodies; + b3AlignedObjectArray m_inertias; + b3AlignedObjectArray m_aabbWorldSpace; + + b3DynamicBvhBroadphase* m_bp; + b3CpuNarrowPhase* m_np; + b3Config m_config; +}; + + +b3CpuRigidBodyPipeline::b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config) +{ + m_data = new b3CpuRigidBodyPipelineInternalData; + m_data->m_np = narrowphase; + m_data->m_bp = broadphaseDbvt; + m_data->m_config = config; +} + +b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline() +{ + delete m_data; +} + +void b3CpuRigidBodyPipeline::updateAabbWorldSpace() +{ + + for (int i=0;igetNumBodies();i++) + { + b3RigidBodyData* body = &m_data->m_rigidBodies[i]; + b3Float4 position = body->m_pos; + b3Quat orientation = body->m_quat; + + int collidableIndex = body->m_collidableIdx; + b3Collidable& collidable = m_data->m_np->getCollidableCpu(collidableIndex); + int shapeIndex = collidable.m_shapeIndex; + + if (shapeIndex>=0) + { + + + b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex); + b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i]; + float margin=0.f; + b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&worldAabb.m_minVec,&worldAabb.m_maxVec); + m_data->m_bp->setAabb(i,worldAabb.m_minVec,worldAabb.m_maxVec,0); + } + } +} + +void b3CpuRigidBodyPipeline::computeOverlappingPairs() +{ + int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs(); + m_data->m_bp->calculateOverlappingPairs(); + numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs(); + printf("numPairs=%d\n",numPairs); +} + +void b3CpuRigidBodyPipeline::computeContactPoints() +{ + + b3AlignedObjectArray& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray(); + + m_data->m_np->computeContacts(pairs,m_data->m_aabbWorldSpace, m_data->m_rigidBodies); + +} +void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime) +{ + + //update world space aabb's + updateAabbWorldSpace(); + + //compute overlapping pairs + computeOverlappingPairs(); + + //compute contacts + computeContactPoints(); + + //solve contacts + + //update transforms + integrate(deltaTime); + + +} + + +static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1, + const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1) +{ + return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1); +} + + +static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1, + b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1) +{ + linear = -n; + angular0 = -b3Cross(r0, n); + angular1 = b3Cross(r1, n); +} + + + +static inline void b3SolveContact(b3ContactConstraint4& cs, + const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4]) +{ + + b3Vector3 dLinVelA; dLinVelA.setZero(); + b3Vector3 dAngVelA; dAngVelA.setZero(); + b3Vector3 dLinVelB; dLinVelB.setZero(); + b3Vector3 dAngVelB; dAngVelB.setZero(); + + for(int ic=0; ic<4; ic++) + { + // dont necessary because this makes change to 0 + if( cs.m_jacCoeffInv[ic] == 0.f ) continue; + + { + b3Vector3 angular0, angular1, linear; + b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA; + b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB; + b3SetLinearAndAngular( (const b3Vector3 &)-cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, linear, angular0, angular1 ); + + float rambdaDt = b3CalcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic]; + rambdaDt *= cs.m_jacCoeffInv[ic]; + + { + float prevSum = cs.m_appliedRambdaDt[ic]; + float updated = prevSum; + updated += rambdaDt; + updated = b3Max( updated, minRambdaDt[ic] ); + updated = b3Min( updated, maxRambdaDt[ic] ); + rambdaDt = updated - prevSum; + cs.m_appliedRambdaDt[ic] = updated; + } + + b3Vector3 linImp0 = invMassA*linear*rambdaDt; + b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; + b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; + b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; +#ifdef _WIN32 + b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp1.getX())); +#endif + { + linVelA += linImp0; + angVelA += angImp0; + linVelB += linImp1; + angVelB += angImp1; + } + } + } + + +} + + + + + +static inline void b3SolveFriction(b3ContactConstraint4& cs, + const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4]) +{ + + if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return; + const b3Vector3& center = (const b3Vector3&)cs.m_center; + + b3Vector3 n = -(const b3Vector3&)cs.m_linear; + + b3Vector3 tangent[2]; + + b3PlaneSpace1 (n, tangent[0],tangent[1]); + + b3Vector3 angular0, angular1, linear; + b3Vector3 r0 = center - posA; + b3Vector3 r1 = center - posB; + for(int i=0; i<2; i++) + { + b3SetLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 ); + float rambdaDt = b3CalcRelVel(linear, -linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB ); + rambdaDt *= cs.m_fJacCoeffInv[i]; + + { + float prevSum = cs.m_fAppliedRambdaDt[i]; + float updated = prevSum; + updated += rambdaDt; + updated = b3Max( updated, minRambdaDt[i] ); + updated = b3Min( updated, maxRambdaDt[i] ); + rambdaDt = updated - prevSum; + cs.m_fAppliedRambdaDt[i] = updated; + } + + b3Vector3 linImp0 = invMassA*linear*rambdaDt; + b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; + b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; + b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; +#ifdef _WIN32 + b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp1.getX())); +#endif + linVelA += linImp0; + angVelA += angImp0; + linVelB += linImp1; + angVelB += angImp1; + } + + { // angular damping for point constraint + b3Vector3 ab = ( posB - posA ).normalized(); + b3Vector3 ac = ( center - posA ).normalized(); + if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) + { + float angNA = b3Dot( n, angVelA ); + float angNB = b3Dot( n, angVelB ); + + angVelA -= (angNA*0.1f)*n; + angVelB -= (angNB*0.1f)*n; + } + } + +} + + + + + +struct b3SolveTask// : public ThreadPool::Task +{ + b3SolveTask(b3AlignedObjectArray& bodies, + b3AlignedObjectArray& shapes, + b3AlignedObjectArray& constraints, + int start, int nConstraints, + int maxNumBatches, + b3AlignedObjectArray* wgUsedBodies, int curWgidx + ) + : m_bodies( bodies ), m_shapes( shapes ), m_constraints( constraints ), + m_wgUsedBodies(wgUsedBodies),m_curWgidx(curWgidx), +m_start( start ), + m_nConstraints( nConstraints ), + m_solveFriction( true ), + m_maxNumBatches(maxNumBatches) + {} + + unsigned short int getType(){ return 0; } + + void run(int tIdx) + { + b3AlignedObjectArray usedBodies; + //printf("run..............\n"); + + + for (int bb=0;bb=0; ic--) + //for(int ic=0; ic& m_bodies; + b3AlignedObjectArray& m_shapes; + b3AlignedObjectArray& m_constraints; + b3AlignedObjectArray* m_wgUsedBodies; + int m_curWgidx; + int m_start; + int m_nConstraints; + bool m_solveFriction; + int m_maxNumBatches; +}; + +void b3CpuRigidBodyPipeline::solveContactConstraints() +{ + int m_nIterations = 4; + + b3AlignedObjectArray contactConstraints; +// const b3AlignedObjectArray& contacts = m_data->m_np->getContacts(); + int n = contactConstraints.size(); + //convert contacts... + + + + int maxNumBatches = 250; + + for(int iter=0; iterm_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0); + task.m_solveFriction = false; + task.run(0); + } + + for(int iter=0; iterm_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0); + task.m_solveFriction = true; + task.run(0); + } +} + +void b3CpuRigidBodyPipeline::integrate(float deltaTime) +{ + float angDamping=0.f; + b3Vector3 gravityAcceleration=b3MakeVector3(0,-9,0); + + //integrate transforms (external forces/gravity should be moved into constraint solver) + for (int i=0;im_rigidBodies.size();i++) + { + b3IntegrateTransform(&m_data->m_rigidBodies[i],deltaTime,angDamping,gravityAcceleration); + } + +} + +int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData) +{ + b3RigidBodyData body; + int bodyIndex = m_data->m_rigidBodies.size(); + body.m_invMass = mass ? 1.f/mass : 0.f; + body.m_angVel.setValue(0,0,0); + body.m_collidableIdx = collidableIndex; + body.m_frictionCoeff = 0.3f; + body.m_linVel.setValue(0,0,0); + body.m_pos.setValue(position[0],position[1],position[2]); + body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]); + body.m_restituitionCoeff = 0.f; + + m_data->m_rigidBodies.push_back(body); + + + if (collidableIndex>=0) + { + b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand(); + + b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex); + b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]); + b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]); + + b3Scalar margin = 0.01f; + b3Transform t; + t.setIdentity(); + t.setOrigin(b3MakeVector3(position[0],position[1],position[2])); + t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3])); + b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec); + + m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1); +// b3Vector3 aabbMin,aabbMax; + // m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax); + + } else + { + b3Error("registerPhysicsInstance using invalid collidableIndex\n"); + } + + return bodyIndex; +} + + +const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const +{ + return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0; +} + +int b3CpuRigidBodyPipeline::getNumBodies() const +{ + return m_data->m_rigidBodies.size(); +} diff --git a/extern/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h b/extern/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h new file mode 100644 index 000000000000..2f3c2ae77e03 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h @@ -0,0 +1,67 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + +#ifndef B3_CPU_RIGIDBODY_PIPELINE_H +#define B3_CPU_RIGIDBODY_PIPELINE_H + + + +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h" + +class b3CpuRigidBodyPipeline +{ +protected: + struct b3CpuRigidBodyPipelineInternalData* m_data; + + int allocateCollidable(); + +public: + + + b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const struct b3Config& config); + virtual ~b3CpuRigidBodyPipeline(); + + virtual void stepSimulation(float deltaTime); + virtual void integrate(float timeStep); + virtual void updateAabbWorldSpace(); + virtual void computeOverlappingPairs(); + virtual void computeContactPoints(); + virtual void solveContactConstraints(); + + int registerConvexPolyhedron(class b3ConvexUtility* convex); + + int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData); + void writeAllInstancesToGpu(); + void copyConstraintsToHost(); + void setGravity(const float* grav); + void reset(); + + int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold); + int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold); + void removeConstraintByUid(int uid); + + void addConstraint(class b3TypedConstraint* constraint); + void removeConstraint(b3TypedConstraint* constraint); + + void castRays(const b3AlignedObjectArray& rays, b3AlignedObjectArray& hitResults); + + const struct b3RigidBodyData* getBodyBuffer() const; + + int getNumBodies() const; + +}; + +#endif //B3_CPU_RIGIDBODY_PIPELINE_H \ No newline at end of file diff --git a/extern/bullet/src/Bullet3Dynamics/premake4.lua b/extern/bullet/src/Bullet3Dynamics/premake4.lua new file mode 100644 index 000000000000..e05b2cd19486 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/premake4.lua @@ -0,0 +1,18 @@ + project "Bullet3Dynamics" + + language "C++" + + kind "StaticLib" + + includedirs { + ".." + } + + if os.is("Linux") then + buildoptions{"-fPIC"} + end + + files { + "**.cpp", + "**.h" + } \ No newline at end of file diff --git a/extern/bullet/src/Bullet3Dynamics/shared/b3ContactConstraint4.h b/extern/bullet/src/Bullet3Dynamics/shared/b3ContactConstraint4.h new file mode 100644 index 000000000000..68cf65e3129e --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/shared/b3ContactConstraint4.h @@ -0,0 +1,34 @@ +#ifndef B3_CONTACT_CONSTRAINT5_H +#define B3_CONTACT_CONSTRAINT5_H + +#include "Bullet3Common/shared/b3Float4.h" + +typedef struct b3ContactConstraint4 b3ContactConstraint4_t; + + +struct b3ContactConstraint4 +{ + + b3Float4 m_linear;//normal? + b3Float4 m_worldPos[4]; + b3Float4 m_center; // friction + float m_jacCoeffInv[4]; + float m_b[4]; + float m_appliedRambdaDt[4]; + float m_fJacCoeffInv[2]; // friction + float m_fAppliedRambdaDt[2]; // friction + + unsigned int m_bodyA; + unsigned int m_bodyB; + int m_batchIdx; + unsigned int m_paddings; + +}; + +//inline void setFrictionCoeff(float value) { m_linear[3] = value; } +inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint) +{ + return constraint->m_linear.w; +} + +#endif //B3_CONTACT_CONSTRAINT5_H diff --git a/extern/bullet/src/Bullet3Dynamics/shared/b3ConvertConstraint4.h b/extern/bullet/src/Bullet3Dynamics/shared/b3ConvertConstraint4.h new file mode 100644 index 000000000000..805a2bd3ea09 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/shared/b3ConvertConstraint4.h @@ -0,0 +1,153 @@ + + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" +#include "Bullet3Dynamics/shared/b3ContactConstraint4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + + +void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q); + void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q) +{ + if (b3Fabs(n.z) > 0.70710678f) { + // choose p in y-z plane + float a = n.y*n.y + n.z*n.z; + float k = 1.f/sqrt(a); + p[0].x = 0; + p[0].y = -n.z*k; + p[0].z = n.y*k; + // set q = n x p + q[0].x = a*k; + q[0].y = -n.x*p[0].z; + q[0].z = n.x*p[0].y; + } + else { + // choose p in x-y plane + float a = n.x*n.x + n.y*n.y; + float k = 1.f/sqrt(a); + p[0].x = -n.y*k; + p[0].y = n.x*k; + p[0].z = 0; + // set q = n x p + q[0].x = -n.z*p[0].y; + q[0].y = n.z*p[0].x; + q[0].z = a*k; + } +} + + + +void setLinearAndAngular( b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1) +{ + *linear = b3MakeFloat4(n.x,n.y,n.z,0.f); + *angular0 = b3Cross3(r0, n); + *angular1 = -b3Cross3(r1, n); +} + + +float calcRelVel( b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0, + b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1 ) +{ + return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1); +} + + +float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1, + float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1) +{ + // linear0,1 are normlized + float jmj0 = invMass0;//b3Dot3F4(linear0, linear0)*invMass0; + float jmj1 = b3Dot3F4(mtMul3(angular0,*invInertia0), angular0); + float jmj2 = invMass1;//b3Dot3F4(linear1, linear1)*invMass1; + float jmj3 = b3Dot3F4(mtMul3(angular1,*invInertia1), angular1); + return -1.f/(jmj0+jmj1+jmj2+jmj3); +} + + +void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA, + b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB, + __global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff, + b3ContactConstraint4_t* dstC ) +{ + dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit); + dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit); + + float dtInv = 1.f/dt; + for(int ic=0; ic<4; ic++) + { + dstC->m_appliedRambdaDt[ic] = 0.f; + } + dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f; + + + dstC->m_linear = src->m_worldNormalOnB; + dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() ); + for(int ic=0; ic<4; ic++) + { + b3Float4 r0 = src->m_worldPosB[ic] - posA; + b3Float4 r1 = src->m_worldPosB[ic] - posB; + + if( ic >= src->m_worldNormalOnB.w )//npoints + { + dstC->m_jacCoeffInv[ic] = 0.f; + continue; + } + + float relVelN; + { + b3Float4 linear, angular0, angular1; + setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1); + + dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1, + invMassA, &invInertiaA, invMassB, &invInertiaB ); + + relVelN = calcRelVel(linear, -linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB); + + float e = 0.f;//src->getRestituitionCoeff(); + if( relVelN*relVelN < 0.004f ) e = 0.f; + + dstC->m_b[ic] = e*relVelN; + //float penetration = src->m_worldPosB[ic].w; + dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv; + dstC->m_appliedRambdaDt[ic] = 0.f; + } + } + + if( src->m_worldNormalOnB.w > 0 )//npoints + { // prepare friction + b3Float4 center = b3MakeFloat4(0.f,0.f,0.f,0.f); + for(int i=0; im_worldNormalOnB.w; i++) + center += src->m_worldPosB[i]; + center /= (float)src->m_worldNormalOnB.w; + + b3Float4 tangent[2]; + b3PlaneSpace1(src->m_worldNormalOnB,&tangent[0],&tangent[1]); + + b3Float4 r[2]; + r[0] = center - posA; + r[1] = center - posB; + + for(int i=0; i<2; i++) + { + b3Float4 linear, angular0, angular1; + setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1); + + dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1, + invMassA, &invInertiaA, invMassB, &invInertiaB ); + dstC->m_fAppliedRambdaDt[i] = 0.f; + } + dstC->m_center = center; + } + + for(int i=0; i<4; i++) + { + if( im_worldNormalOnB.w ) + { + dstC->m_worldPos[i] = src->m_worldPosB[i]; + } + else + { + dstC->m_worldPos[i] = b3MakeFloat4(0.f,0.f,0.f,0.f); + } + } +} diff --git a/extern/bullet/src/Bullet3Dynamics/shared/b3Inertia.h b/extern/bullet/src/Bullet3Dynamics/shared/b3Inertia.h new file mode 100644 index 000000000000..96fe9f8b3983 --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/shared/b3Inertia.h @@ -0,0 +1,15 @@ + + +#ifndef B3_INERTIA_H +#define B3_INERTIA_H + +#include "Bullet3Common/shared/b3Mat3x3.h" + +struct b3Inertia +{ + b3Mat3x3 m_invInertiaWorld; + b3Mat3x3 m_initInvInertia; +}; + + +#endif //B3_INERTIA_H \ No newline at end of file diff --git a/extern/bullet/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h b/extern/bullet/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h new file mode 100644 index 000000000000..e96f90d3f32d --- /dev/null +++ b/extern/bullet/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h @@ -0,0 +1,113 @@ + + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + + + +inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration) +{ + + if (bodies[nodeID].m_invMass != 0.f) + { + float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f); + + //angular velocity + { + b3Float4 axis; + //add some hardcoded angular damping + bodies[nodeID].m_angVel.x *= angularDamping; + bodies[nodeID].m_angVel.y *= angularDamping; + bodies[nodeID].m_angVel.z *= angularDamping; + + b3Float4 angvel = bodies[nodeID].m_angVel; + + float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel)); + + //limit the angular motion + if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD) + { + fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep; + } + if(fAngle < 0.001f) + { + // use Taylor's expansions of sync function + axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle); + } + else + { + // sync(fAngle) = sin(c*fAngle)/t + axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle); + } + + b3Quat dorn; + dorn.x = axis.x; + dorn.y = axis.y; + dorn.z = axis.z; + dorn.w = b3Cos(fAngle * timeStep * 0.5f); + b3Quat orn0 = bodies[nodeID].m_quat; + b3Quat predictedOrn = b3QuatMul(dorn, orn0); + predictedOrn = b3QuatNormalized(predictedOrn); + bodies[nodeID].m_quat=predictedOrn; + } + //linear velocity + bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep; + + //apply gravity + bodies[nodeID].m_linVel += gravityAcceleration * timeStep; + + } + +} + +inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration) +{ + float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f); + + if( (body->m_invMass != 0.f)) + { + //angular velocity + { + b3Float4 axis; + //add some hardcoded angular damping + body->m_angVel.x *= angularDamping; + body->m_angVel.y *= angularDamping; + body->m_angVel.z *= angularDamping; + + b3Float4 angvel = body->m_angVel; + float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel)); + //limit the angular motion + if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD) + { + fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep; + } + if(fAngle < 0.001f) + { + // use Taylor's expansions of sync function + axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle); + } + else + { + // sync(fAngle) = sin(c*fAngle)/t + axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle); + } + b3Quat dorn; + dorn.x = axis.x; + dorn.y = axis.y; + dorn.z = axis.z; + dorn.w = b3Cos(fAngle * timeStep * 0.5f); + b3Quat orn0 = body->m_quat; + + b3Quat predictedOrn = b3QuatMul(dorn, orn0); + predictedOrn = b3QuatNormalized(predictedOrn); + body->m_quat=predictedOrn; + } + + //apply gravity + body->m_linVel += gravityAcceleration * timeStep; + + //linear velocity + body->m_pos += body->m_linVel * timeStep; + + } + +} diff --git a/extern/bullet/src/Bullet3Geometry/CMakeLists.txt b/extern/bullet/src/Bullet3Geometry/CMakeLists.txt new file mode 100644 index 000000000000..82068727054d --- /dev/null +++ b/extern/bullet/src/Bullet3Geometry/CMakeLists.txt @@ -0,0 +1,47 @@ + +INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/src +) + +SET(Bullet3Geometry_SRCS + b3ConvexHullComputer.cpp + b3GeometryUtil.cpp +) + +SET(Bullet3Geometry_HDRS + b3AabbUtil.h + b3ConvexHullComputer.h + b3GeometryUtil.h + b3GrahamScan2dConvexHull.h +) + +ADD_LIBRARY(Bullet3Geometry ${Bullet3Geometry_SRCS} ${Bullet3Geometry_HDRS}) +if (BUILD_SHARED_LIBS) + target_link_libraries(Bullet3Geometry Bullet3Common) +endif() +SET_TARGET_PROPERTIES(Bullet3Geometry PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(Bullet3Geometry PROPERTIES SOVERSION ${BULLET_VERSION}) + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + #FILES_MATCHING requires CMake 2.6 + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS Bullet3Geometry DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS Bullet3Geometry + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN +".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(Bullet3Geometry PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(Bullet3Geometry PROPERTIES PUBLIC_HEADER "${Bullet3Geometry_HDRS}") + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/extern/bullet/src/Bullet3Geometry/b3AabbUtil.h b/extern/bullet/src/Bullet3Geometry/b3AabbUtil.h new file mode 100644 index 000000000000..4c72d5bbfc04 --- /dev/null +++ b/extern/bullet/src/Bullet3Geometry/b3AabbUtil.h @@ -0,0 +1,232 @@ +/* +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. +*/ + + + +#ifndef B3_AABB_UTIL2 +#define B3_AABB_UTIL2 + +#include "Bullet3Common/b3Transform.h" +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3MinMax.h" + + + +B3_FORCE_INLINE void b3AabbExpand (b3Vector3& aabbMin, + b3Vector3& aabbMax, + const b3Vector3& expansionMin, + const b3Vector3& expansionMax) +{ + aabbMin = aabbMin + expansionMin; + aabbMax = aabbMax + expansionMax; +} + +/// conservative test for overlap between two aabbs +B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aabbMax1, + const b3Vector3 &point) +{ + bool overlap = true; + overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap; + overlap = (aabbMin1.getZ() > point.getZ() || aabbMax1.getZ() < point.getZ()) ? false : overlap; + overlap = (aabbMin1.getY() > point.getY() || aabbMax1.getY() < point.getY()) ? false : overlap; + return overlap; +} + + +/// conservative test for overlap between two aabbs +B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aabbMax1, + const b3Vector3 &aabbMin2, const b3Vector3 &aabbMax2) +{ + bool overlap = true; + overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap; + overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap; + overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap; + return overlap; +} + +/// conservative test for overlap between triangle and aabb +B3_FORCE_INLINE bool b3TestTriangleAgainstAabb2(const b3Vector3 *vertices, + const b3Vector3 &aabbMin, const b3Vector3 &aabbMax) +{ + const b3Vector3 &p1 = vertices[0]; + const b3Vector3 &p2 = vertices[1]; + const b3Vector3 &p3 = vertices[2]; + + if (b3Min(b3Min(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false; + if (b3Max(b3Max(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false; + + if (b3Min(b3Min(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false; + if (b3Max(b3Max(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false; + + if (b3Min(b3Min(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false; + if (b3Max(b3Max(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false; + return true; +} + + +B3_FORCE_INLINE int b3Outcode(const b3Vector3& p,const b3Vector3& halfExtent) +{ + return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) | + (p.getX() > halfExtent.getX() ? 0x08 : 0x0) | + (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) | + (p.getY() > halfExtent.getY() ? 0x10 : 0x0) | + (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) | + (p.getZ() > halfExtent.getZ() ? 0x20 : 0x0); +} + + + +B3_FORCE_INLINE bool b3RayAabb2(const b3Vector3& rayFrom, + const b3Vector3& rayInvDirection, + const unsigned int raySign[3], + const b3Vector3 bounds[2], + b3Scalar& tmin, + b3Scalar lambda_min, + b3Scalar lambda_max) +{ + b3Scalar tmax, tymin, tymax, tzmin, tzmax; + tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX(); + tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX(); + tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY(); + tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY(); + + if ( (tmin > tymax) || (tymin > tmax) ) + return false; + + if (tymin > tmin) + tmin = tymin; + + if (tymax < tmax) + tmax = tymax; + + tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ(); + tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ(); + + if ( (tmin > tzmax) || (tzmin > tmax) ) + return false; + if (tzmin > tmin) + tmin = tzmin; + if (tzmax < tmax) + tmax = tzmax; + return ( (tmin < lambda_max) && (tmax > lambda_min) ); +} + +B3_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom, + const b3Vector3& rayTo, + const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + b3Scalar& param, b3Vector3& normal) +{ + b3Vector3 aabbHalfExtent = (aabbMax-aabbMin)* b3Scalar(0.5); + b3Vector3 aabbCenter = (aabbMax+aabbMin)* b3Scalar(0.5); + b3Vector3 source = rayFrom - aabbCenter; + b3Vector3 target = rayTo - aabbCenter; + int sourceOutcode = b3Outcode(source,aabbHalfExtent); + int targetOutcode = b3Outcode(target,aabbHalfExtent); + if ((sourceOutcode & targetOutcode) == 0x0) + { + b3Scalar lambda_enter = b3Scalar(0.0); + b3Scalar lambda_exit = param; + b3Vector3 r = target - source; + int i; + b3Scalar normSign = 1; + b3Vector3 hitNormal = b3MakeVector3(0,0,0); + int bit=1; + + for (int j=0;j<2;j++) + { + for (i = 0; i != 3; ++i) + { + if (sourceOutcode & bit) + { + b3Scalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i]; + if (lambda_enter <= lambda) + { + lambda_enter = lambda; + hitNormal.setValue(0,0,0); + hitNormal[i] = normSign; + } + } + else if (targetOutcode & bit) + { + b3Scalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i]; + b3SetMin(lambda_exit, lambda); + } + bit<<=1; + } + normSign = b3Scalar(-1.); + } + if (lambda_enter <= lambda_exit) + { + param = lambda_enter; + normal = hitNormal; + return true; + } + } + return false; +} + + + +B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& halfExtents, b3Scalar margin,const b3Transform& t,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut) +{ + b3Vector3 halfExtentsWithMargin = halfExtents+b3MakeVector3(margin,margin,margin); + b3Matrix3x3 abs_b = t.getBasis().absolute(); + b3Vector3 center = t.getOrigin(); + b3Vector3 extent = halfExtentsWithMargin.dot3( abs_b[0], abs_b[1], abs_b[2] ); + aabbMinOut = center - extent; + aabbMaxOut = center + extent; +} + + +B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& localAabbMin,const b3Vector3& localAabbMax, b3Scalar margin,const b3Transform& trans,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut) +{ + //b3Assert(localAabbMin.getX() <= localAabbMax.getX()); + //b3Assert(localAabbMin.getY() <= localAabbMax.getY()); + //b3Assert(localAabbMin.getZ() <= localAabbMax.getZ()); + b3Vector3 localHalfExtents = b3Scalar(0.5)*(localAabbMax-localAabbMin); + localHalfExtents+=b3MakeVector3(margin,margin,margin); + + b3Vector3 localCenter = b3Scalar(0.5)*(localAabbMax+localAabbMin); + b3Matrix3x3 abs_b = trans.getBasis().absolute(); + b3Vector3 center = trans(localCenter); + b3Vector3 extent = localHalfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] ); + aabbMinOut = center-extent; + aabbMaxOut = center+extent; +} + +#define B3_USE_BANCHLESS 1 +#ifdef B3_USE_BANCHLESS + //This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360) + B3_FORCE_INLINE unsigned b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) + { + return static_cast(b3Select((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) + & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) + & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), + 1, 0)); + } +#else + B3_FORCE_INLINE bool b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) + { + bool overlap = true; + overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; + overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; + overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; + return overlap; + } +#endif //B3_USE_BANCHLESS + +#endif //B3_AABB_UTIL2 + + diff --git a/extern/bullet/src/Bullet3Geometry/b3ConvexHullComputer.cpp b/extern/bullet/src/Bullet3Geometry/b3ConvexHullComputer.cpp new file mode 100644 index 000000000000..18835c38d5a9 --- /dev/null +++ b/extern/bullet/src/Bullet3Geometry/b3ConvexHullComputer.cpp @@ -0,0 +1,2755 @@ +/* +Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net + +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 + +#include "b3ConvexHullComputer.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3MinMax.h" +#include "Bullet3Common/b3Vector3.h" + +#ifdef __GNUC__ + #include + typedef int32_t btInt32_t; + typedef int64_t btInt64_t; + typedef uint32_t btUint32_t; + typedef uint64_t btUint64_t; +#elif defined(_MSC_VER) + typedef __int32 btInt32_t; + typedef __int64 btInt64_t; + typedef unsigned __int32 btUint32_t; + typedef unsigned __int64 btUint64_t; +#else + typedef int btInt32_t; + typedef long long int btInt64_t; + typedef unsigned int btUint32_t; + typedef unsigned long long int btUint64_t; +#endif + + +//The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines +//#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL)) // || (defined(__ICL) && defined(_M_X64)) bug in Intel compiler, disable inline assembly +// #define USE_X86_64_ASM +//#endif + + +//#define DEBUG_CONVEX_HULL +//#define SHOW_ITERATIONS + +#if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS) + #include +#endif + +// Convex hull implementation based on Preparata and Hong +// Ole Kniemeyer, MAXON Computer GmbH +class b3ConvexHullInternal +{ + public: + + class Point64 + { + public: + btInt64_t x; + btInt64_t y; + btInt64_t z; + + Point64(btInt64_t x, btInt64_t y, btInt64_t z): x(x), y(y), z(z) + { + } + + bool isZero() + { + return (x == 0) && (y == 0) && (z == 0); + } + + btInt64_t dot(const Point64& b) const + { + return x * b.x + y * b.y + z * b.z; + } + }; + + class Point32 + { + public: + btInt32_t x; + btInt32_t y; + btInt32_t z; + int index; + + Point32() + { + } + + Point32(btInt32_t x, btInt32_t y, btInt32_t z): x(x), y(y), z(z), index(-1) + { + } + + bool operator==(const Point32& b) const + { + return (x == b.x) && (y == b.y) && (z == b.z); + } + + bool operator!=(const Point32& b) const + { + return (x != b.x) || (y != b.y) || (z != b.z); + } + + bool isZero() + { + return (x == 0) && (y == 0) && (z == 0); + } + + Point64 cross(const Point32& b) const + { + return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); + } + + Point64 cross(const Point64& b) const + { + return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); + } + + btInt64_t dot(const Point32& b) const + { + return x * b.x + y * b.y + z * b.z; + } + + btInt64_t dot(const Point64& b) const + { + return x * b.x + y * b.y + z * b.z; + } + + Point32 operator+(const Point32& b) const + { + return Point32(x + b.x, y + b.y, z + b.z); + } + + Point32 operator-(const Point32& b) const + { + return Point32(x - b.x, y - b.y, z - b.z); + } + }; + + class Int128 + { + public: + btUint64_t low; + btUint64_t high; + + Int128() + { + } + + Int128(btUint64_t low, btUint64_t high): low(low), high(high) + { + } + + Int128(btUint64_t low): low(low), high(0) + { + } + + Int128(btInt64_t value): low(value), high((value >= 0) ? 0 : (btUint64_t) -1LL) + { + } + + static Int128 mul(btInt64_t a, btInt64_t b); + + static Int128 mul(btUint64_t a, btUint64_t b); + + Int128 operator-() const + { + return Int128((btUint64_t) -(btInt64_t)low, ~high + (low == 0)); + } + + Int128 operator+(const Int128& b) const + { +#ifdef USE_X86_64_ASM + Int128 result; + __asm__ ("addq %[bl], %[rl]\n\t" + "adcq %[bh], %[rh]\n\t" + : [rl] "=r" (result.low), [rh] "=r" (result.high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc" ); + return result; +#else + btUint64_t lo = low + b.low; + return Int128(lo, high + b.high + (lo < low)); +#endif + } + + Int128 operator-(const Int128& b) const + { +#ifdef USE_X86_64_ASM + Int128 result; + __asm__ ("subq %[bl], %[rl]\n\t" + "sbbq %[bh], %[rh]\n\t" + : [rl] "=r" (result.low), [rh] "=r" (result.high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc" ); + return result; +#else + return *this + -b; +#endif + } + + Int128& operator+=(const Int128& b) + { +#ifdef USE_X86_64_ASM + __asm__ ("addq %[bl], %[rl]\n\t" + "adcq %[bh], %[rh]\n\t" + : [rl] "=r" (low), [rh] "=r" (high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc" ); +#else + btUint64_t lo = low + b.low; + if (lo < low) + { + ++high; + } + low = lo; + high += b.high; +#endif + return *this; + } + + Int128& operator++() + { + if (++low == 0) + { + ++high; + } + return *this; + } + + Int128 operator*(btInt64_t b) const; + + b3Scalar toScalar() const + { + return ((btInt64_t) high >= 0) ? b3Scalar(high) * (b3Scalar(0x100000000LL) * b3Scalar(0x100000000LL)) + b3Scalar(low) + : -(-*this).toScalar(); + } + + int getSign() const + { + return ((btInt64_t) high < 0) ? -1 : (high || low) ? 1 : 0; + } + + bool operator<(const Int128& b) const + { + return (high < b.high) || ((high == b.high) && (low < b.low)); + } + + int ucmp(const Int128&b) const + { + if (high < b.high) + { + return -1; + } + if (high > b.high) + { + return 1; + } + if (low < b.low) + { + return -1; + } + if (low > b.low) + { + return 1; + } + return 0; + } + }; + + + class Rational64 + { + private: + btUint64_t m_numerator; + btUint64_t m_denominator; + int sign; + + public: + Rational64(btInt64_t numerator, btInt64_t denominator) + { + if (numerator > 0) + { + sign = 1; + m_numerator = (btUint64_t) numerator; + } + else if (numerator < 0) + { + sign = -1; + m_numerator = (btUint64_t) -numerator; + } + else + { + sign = 0; + m_numerator = 0; + } + if (denominator > 0) + { + m_denominator = (btUint64_t) denominator; + } + else if (denominator < 0) + { + sign = -sign; + m_denominator = (btUint64_t) -denominator; + } + else + { + m_denominator = 0; + } + } + + bool isNegativeInfinity() const + { + return (sign < 0) && (m_denominator == 0); + } + + bool isNaN() const + { + return (sign == 0) && (m_denominator == 0); + } + + int compare(const Rational64& b) const; + + b3Scalar toScalar() const + { + return sign * ((m_denominator == 0) ? B3_INFINITY : (b3Scalar) m_numerator / m_denominator); + } + }; + + + class Rational128 + { + private: + Int128 numerator; + Int128 denominator; + int sign; + bool isInt64; + + public: + Rational128(btInt64_t value) + { + if (value > 0) + { + sign = 1; + this->numerator = value; + } + else if (value < 0) + { + sign = -1; + this->numerator = -value; + } + else + { + sign = 0; + this->numerator = (btUint64_t) 0; + } + this->denominator = (btUint64_t) 1; + isInt64 = true; + } + + Rational128(const Int128& numerator, const Int128& denominator) + { + sign = numerator.getSign(); + if (sign >= 0) + { + this->numerator = numerator; + } + else + { + this->numerator = -numerator; + } + int dsign = denominator.getSign(); + if (dsign >= 0) + { + this->denominator = denominator; + } + else + { + sign = -sign; + this->denominator = -denominator; + } + isInt64 = false; + } + + int compare(const Rational128& b) const; + + int compare(btInt64_t b) const; + + b3Scalar toScalar() const + { + return sign * ((denominator.getSign() == 0) ? B3_INFINITY : numerator.toScalar() / denominator.toScalar()); + } + }; + + class PointR128 + { + public: + Int128 x; + Int128 y; + Int128 z; + Int128 denominator; + + PointR128() + { + } + + PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator): x(x), y(y), z(z), denominator(denominator) + { + } + + b3Scalar xvalue() const + { + return x.toScalar() / denominator.toScalar(); + } + + b3Scalar yvalue() const + { + return y.toScalar() / denominator.toScalar(); + } + + b3Scalar zvalue() const + { + return z.toScalar() / denominator.toScalar(); + } + }; + + + class Edge; + class Face; + + class Vertex + { + public: + Vertex* next; + Vertex* prev; + Edge* edges; + Face* firstNearbyFace; + Face* lastNearbyFace; + PointR128 point128; + Point32 point; + int copy; + + Vertex(): next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1) + { + } + +#ifdef DEBUG_CONVEX_HULL + void print() + { + b3Printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z); + } + + void printGraph(); +#endif + + Point32 operator-(const Vertex& b) const + { + return point - b.point; + } + + Rational128 dot(const Point64& b) const + { + return (point.index >= 0) ? Rational128(point.dot(b)) + : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator); + } + + b3Scalar xvalue() const + { + return (point.index >= 0) ? b3Scalar(point.x) : point128.xvalue(); + } + + b3Scalar yvalue() const + { + return (point.index >= 0) ? b3Scalar(point.y) : point128.yvalue(); + } + + b3Scalar zvalue() const + { + return (point.index >= 0) ? b3Scalar(point.z) : point128.zvalue(); + } + + void receiveNearbyFaces(Vertex* src) + { + if (lastNearbyFace) + { + lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace; + } + else + { + firstNearbyFace = src->firstNearbyFace; + } + if (src->lastNearbyFace) + { + lastNearbyFace = src->lastNearbyFace; + } + for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex) + { + b3Assert(f->nearbyVertex == src); + f->nearbyVertex = this; + } + src->firstNearbyFace = NULL; + src->lastNearbyFace = NULL; + } + }; + + + class Edge + { + public: + Edge* next; + Edge* prev; + Edge* reverse; + Vertex* target; + Face* face; + int copy; + + ~Edge() + { + next = NULL; + prev = NULL; + reverse = NULL; + target = NULL; + face = NULL; + } + + void link(Edge* n) + { + b3Assert(reverse->target == n->reverse->target); + next = n; + n->prev = this; + } + +#ifdef DEBUG_CONVEX_HULL + void print() + { + b3Printf("E%p : %d -> %d, n=%p p=%p (0 %d\t%d\t%d) -> (%d %d %d)", this, reverse->target->point.index, target->point.index, next, prev, + reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z); + } +#endif + }; + + class Face + { + public: + Face* next; + Vertex* nearbyVertex; + Face* nextWithSameNearbyVertex; + Point32 origin; + Point32 dir0; + Point32 dir1; + + Face(): next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL) + { + } + + void init(Vertex* a, Vertex* b, Vertex* c) + { + nearbyVertex = a; + origin = a->point; + dir0 = *b - *a; + dir1 = *c - *a; + if (a->lastNearbyFace) + { + a->lastNearbyFace->nextWithSameNearbyVertex = this; + } + else + { + a->firstNearbyFace = this; + } + a->lastNearbyFace = this; + } + + Point64 getNormal() + { + return dir0.cross(dir1); + } + }; + + template class DMul + { + private: + static btUint32_t high(btUint64_t value) + { + return (btUint32_t) (value >> 32); + } + + static btUint32_t low(btUint64_t value) + { + return (btUint32_t) value; + } + + static btUint64_t mul(btUint32_t a, btUint32_t b) + { + return (btUint64_t) a * (btUint64_t) b; + } + + static void shlHalf(btUint64_t& value) + { + value <<= 32; + } + + static btUint64_t high(Int128 value) + { + return value.high; + } + + static btUint64_t low(Int128 value) + { + return value.low; + } + + static Int128 mul(btUint64_t a, btUint64_t b) + { + return Int128::mul(a, b); + } + + static void shlHalf(Int128& value) + { + value.high = value.low; + value.low = 0; + } + + public: + + static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh) + { + UWord p00 = mul(low(a), low(b)); + UWord p01 = mul(low(a), high(b)); + UWord p10 = mul(high(a), low(b)); + UWord p11 = mul(high(a), high(b)); + UWord p0110 = UWord(low(p01)) + UWord(low(p10)); + p11 += high(p01); + p11 += high(p10); + p11 += high(p0110); + shlHalf(p0110); + p00 += p0110; + if (p00 < p0110) + { + ++p11; + } + resLow = p00; + resHigh = p11; + } + }; + + private: + + class IntermediateHull + { + public: + Vertex* minXy; + Vertex* maxXy; + Vertex* minYx; + Vertex* maxYx; + + IntermediateHull(): minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL) + { + } + + void print(); + }; + + enum Orientation {NONE, CLOCKWISE, COUNTER_CLOCKWISE}; + + template class PoolArray + { + private: + T* array; + int size; + + public: + PoolArray* next; + + PoolArray(int size): size(size), next(NULL) + { + array = (T*) b3AlignedAlloc(sizeof(T) * size, 16); + } + + ~PoolArray() + { + b3AlignedFree(array); + } + + T* init() + { + T* o = array; + for (int i = 0; i < size; i++, o++) + { + o->next = (i+1 < size) ? o + 1 : NULL; + } + return array; + } + }; + + template class Pool + { + private: + PoolArray* arrays; + PoolArray* nextArray; + T* freeObjects; + int arraySize; + + public: + Pool(): arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256) + { + } + + ~Pool() + { + while (arrays) + { + PoolArray* p = arrays; + arrays = p->next; + p->~PoolArray(); + b3AlignedFree(p); + } + } + + void reset() + { + nextArray = arrays; + freeObjects = NULL; + } + + void setArraySize(int arraySize) + { + this->arraySize = arraySize; + } + + T* newObject() + { + T* o = freeObjects; + if (!o) + { + PoolArray* p = nextArray; + if (p) + { + nextArray = p->next; + } + else + { + p = new(b3AlignedAlloc(sizeof(PoolArray), 16)) PoolArray(arraySize); + p->next = arrays; + arrays = p; + } + o = p->init(); + } + freeObjects = o->next; + return new(o) T(); + }; + + void freeObject(T* object) + { + object->~T(); + object->next = freeObjects; + freeObjects = object; + } + }; + + b3Vector3 scaling; + b3Vector3 center; + Pool vertexPool; + Pool edgePool; + Pool facePool; + b3AlignedObjectArray originalVertices; + int mergeStamp; + int minAxis; + int medAxis; + int maxAxis; + int usedEdgePairs; + int maxUsedEdgePairs; + + static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t); + Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot); + void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1); + + Edge* newEdgePair(Vertex* from, Vertex* to); + + void removeEdgePair(Edge* edge) + { + Edge* n = edge->next; + Edge* r = edge->reverse; + + b3Assert(edge->target && r->target); + + if (n != edge) + { + n->prev = edge->prev; + edge->prev->next = n; + r->target->edges = n; + } + else + { + r->target->edges = NULL; + } + + n = r->next; + + if (n != r) + { + n->prev = r->prev; + r->prev->next = n; + edge->target->edges = n; + } + else + { + edge->target->edges = NULL; + } + + edgePool.freeObject(edge); + edgePool.freeObject(r); + usedEdgePairs--; + } + + void computeInternal(int start, int end, IntermediateHull& result); + + bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1); + + void merge(IntermediateHull& h0, IntermediateHull& h1); + + b3Vector3 toBtVector(const Point32& v); + + b3Vector3 getBtNormal(Face* face); + + bool shiftFace(Face* face, b3Scalar amount, b3AlignedObjectArray stack); + + public: + Vertex* vertexList; + + void compute(const void* coords, bool doubleCoords, int stride, int count); + + b3Vector3 getCoordinates(const Vertex* v); + + b3Scalar shrink(b3Scalar amount, b3Scalar clampAmount); +}; + + +b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::operator*(btInt64_t b) const +{ + bool negative = (btInt64_t) high < 0; + Int128 a = negative ? -*this : *this; + if (b < 0) + { + negative = !negative; + b = -b; + } + Int128 result = mul(a.low, (btUint64_t) b); + result.high += a.high * (btUint64_t) b; + return negative ? -result : result; +} + +b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::mul(btInt64_t a, btInt64_t b) +{ + Int128 result; + +#ifdef USE_X86_64_ASM + __asm__ ("imulq %[b]" + : "=a" (result.low), "=d" (result.high) + : "0"(a), [b] "r"(b) + : "cc" ); + return result; + +#else + bool negative = a < 0; + if (negative) + { + a = -a; + } + if (b < 0) + { + negative = !negative; + b = -b; + } + DMul::mul((btUint64_t) a, (btUint64_t) b, result.low, result.high); + return negative ? -result : result; +#endif +} + +b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::mul(btUint64_t a, btUint64_t b) +{ + Int128 result; + +#ifdef USE_X86_64_ASM + __asm__ ("mulq %[b]" + : "=a" (result.low), "=d" (result.high) + : "0"(a), [b] "r"(b) + : "cc" ); + +#else + DMul::mul(a, b, result.low, result.high); +#endif + + return result; +} + +int b3ConvexHullInternal::Rational64::compare(const Rational64& b) const +{ + if (sign != b.sign) + { + return sign - b.sign; + } + else if (sign == 0) + { + return 0; + } + + // return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0; + +#ifdef USE_X86_64_ASM + + int result; + btInt64_t tmp; + btInt64_t dummy; + __asm__ ("mulq %[bn]\n\t" + "movq %%rax, %[tmp]\n\t" + "movq %%rdx, %%rbx\n\t" + "movq %[tn], %%rax\n\t" + "mulq %[bd]\n\t" + "subq %[tmp], %%rax\n\t" + "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator" + "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise + "orq %%rdx, %%rax\n\t" + "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero + "decb %%bh\n\t" // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference) + "shll $16, %%ebx\n\t" // ebx has same sign as difference + : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy) + : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator) + : "%rdx", "cc" ); + return result ? result ^ sign // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero) + // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero) + : 0; + +#else + + return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator)); + +#endif +} + +int b3ConvexHullInternal::Rational128::compare(const Rational128& b) const +{ + if (sign != b.sign) + { + return sign - b.sign; + } + else if (sign == 0) + { + return 0; + } + if (isInt64) + { + return -b.compare(sign * (btInt64_t) numerator.low); + } + + Int128 nbdLow, nbdHigh, dbnLow, dbnHigh; + DMul::mul(numerator, b.denominator, nbdLow, nbdHigh); + DMul::mul(denominator, b.numerator, dbnLow, dbnHigh); + + int cmp = nbdHigh.ucmp(dbnHigh); + if (cmp) + { + return cmp * sign; + } + return nbdLow.ucmp(dbnLow) * sign; +} + +int b3ConvexHullInternal::Rational128::compare(btInt64_t b) const +{ + if (isInt64) + { + btInt64_t a = sign * (btInt64_t) numerator.low; + return (a > b) ? 1 : (a < b) ? -1 : 0; + } + if (b > 0) + { + if (sign <= 0) + { + return -1; + } + } + else if (b < 0) + { + if (sign >= 0) + { + return 1; + } + b = -b; + } + else + { + return sign; + } + + return numerator.ucmp(denominator * b) * sign; +} + + +b3ConvexHullInternal::Edge* b3ConvexHullInternal::newEdgePair(Vertex* from, Vertex* to) +{ + b3Assert(from && to); + Edge* e = edgePool.newObject(); + Edge* r = edgePool.newObject(); + e->reverse = r; + r->reverse = e; + e->copy = mergeStamp; + r->copy = mergeStamp; + e->target = to; + r->target = from; + e->face = NULL; + r->face = NULL; + usedEdgePairs++; + if (usedEdgePairs > maxUsedEdgePairs) + { + maxUsedEdgePairs = usedEdgePairs; + } + return e; +} + +bool b3ConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1) +{ + Vertex* v0 = h0.maxYx; + Vertex* v1 = h1.minYx; + if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y)) + { + b3Assert(v0->point.z < v1->point.z); + Vertex* v1p = v1->prev; + if (v1p == v1) + { + c0 = v0; + if (v1->edges) + { + b3Assert(v1->edges->next == v1->edges); + v1 = v1->edges->target; + b3Assert(v1->edges->next == v1->edges); + } + c1 = v1; + return false; + } + Vertex* v1n = v1->next; + v1p->next = v1n; + v1n->prev = v1p; + if (v1 == h1.minXy) + { + if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y))) + { + h1.minXy = v1n; + } + else + { + h1.minXy = v1p; + } + } + if (v1 == h1.maxXy) + { + if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y))) + { + h1.maxXy = v1n; + } + else + { + h1.maxXy = v1p; + } + } + } + + v0 = h0.maxXy; + v1 = h1.maxXy; + Vertex* v00 = NULL; + Vertex* v10 = NULL; + btInt32_t sign = 1; + + for (int side = 0; side <= 1; side++) + { + btInt32_t dx = (v1->point.x - v0->point.x) * sign; + if (dx > 0) + { + while (true) + { + btInt32_t dy = v1->point.y - v0->point.y; + + Vertex* w0 = side ? v0->next : v0->prev; + if (w0 != v0) + { + btInt32_t dx0 = (w0->point.x - v0->point.x) * sign; + btInt32_t dy0 = w0->point.y - v0->point.y; + if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0)))) + { + v0 = w0; + dx = (v1->point.x - v0->point.x) * sign; + continue; + } + } + + Vertex* w1 = side ? v1->next : v1->prev; + if (w1 != v1) + { + btInt32_t dx1 = (w1->point.x - v1->point.x) * sign; + btInt32_t dy1 = w1->point.y - v1->point.y; + btInt32_t dxn = (w1->point.x - v0->point.x) * sign; + if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1)))) + { + v1 = w1; + dx = dxn; + continue; + } + } + + break; + } + } + else if (dx < 0) + { + while (true) + { + btInt32_t dy = v1->point.y - v0->point.y; + + Vertex* w1 = side ? v1->prev : v1->next; + if (w1 != v1) + { + btInt32_t dx1 = (w1->point.x - v1->point.x) * sign; + btInt32_t dy1 = w1->point.y - v1->point.y; + if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1)))) + { + v1 = w1; + dx = (v1->point.x - v0->point.x) * sign; + continue; + } + } + + Vertex* w0 = side ? v0->prev : v0->next; + if (w0 != v0) + { + btInt32_t dx0 = (w0->point.x - v0->point.x) * sign; + btInt32_t dy0 = w0->point.y - v0->point.y; + btInt32_t dxn = (v1->point.x - w0->point.x) * sign; + if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0)))) + { + v0 = w0; + dx = dxn; + continue; + } + } + + break; + } + } + else + { + btInt32_t x = v0->point.x; + btInt32_t y0 = v0->point.y; + Vertex* w0 = v0; + Vertex* t; + while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0)) + { + w0 = t; + y0 = t->point.y; + } + v0 = w0; + + btInt32_t y1 = v1->point.y; + Vertex* w1 = v1; + while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1)) + { + w1 = t; + y1 = t->point.y; + } + v1 = w1; + } + + if (side == 0) + { + v00 = v0; + v10 = v1; + + v0 = h0.minXy; + v1 = h1.minXy; + sign = -1; + } + } + + v0->prev = v1; + v1->next = v0; + + v00->next = v10; + v10->prev = v00; + + if (h1.minXy->point.x < h0.minXy->point.x) + { + h0.minXy = h1.minXy; + } + if (h1.maxXy->point.x >= h0.maxXy->point.x) + { + h0.maxXy = h1.maxXy; + } + + h0.maxYx = h1.maxYx; + + c0 = v00; + c1 = v10; + + return true; +} + +void b3ConvexHullInternal::computeInternal(int start, int end, IntermediateHull& result) +{ + int n = end - start; + switch (n) + { + case 0: + result.minXy = NULL; + result.maxXy = NULL; + result.minYx = NULL; + result.maxYx = NULL; + return; + case 2: + { + Vertex* v = originalVertices[start]; + Vertex* w = v + 1; + if (v->point != w->point) + { + btInt32_t dx = v->point.x - w->point.x; + btInt32_t dy = v->point.y - w->point.y; + + if ((dx == 0) && (dy == 0)) + { + if (v->point.z > w->point.z) + { + Vertex* t = w; + w = v; + v = t; + } + b3Assert(v->point.z < w->point.z); + v->next = v; + v->prev = v; + result.minXy = v; + result.maxXy = v; + result.minYx = v; + result.maxYx = v; + } + else + { + v->next = w; + v->prev = w; + w->next = v; + w->prev = v; + + if ((dx < 0) || ((dx == 0) && (dy < 0))) + { + result.minXy = v; + result.maxXy = w; + } + else + { + result.minXy = w; + result.maxXy = v; + } + + if ((dy < 0) || ((dy == 0) && (dx < 0))) + { + result.minYx = v; + result.maxYx = w; + } + else + { + result.minYx = w; + result.maxYx = v; + } + } + + Edge* e = newEdgePair(v, w); + e->link(e); + v->edges = e; + + e = e->reverse; + e->link(e); + w->edges = e; + + return; + } + } + // lint -fallthrough + case 1: + { + Vertex* v = originalVertices[start]; + v->edges = NULL; + v->next = v; + v->prev = v; + + result.minXy = v; + result.maxXy = v; + result.minYx = v; + result.maxYx = v; + + return; + } + } + + int split0 = start + n / 2; + Point32 p = originalVertices[split0-1]->point; + int split1 = split0; + while ((split1 < end) && (originalVertices[split1]->point == p)) + { + split1++; + } + computeInternal(start, split0, result); + IntermediateHull hull1; + computeInternal(split1, end, hull1); +#ifdef DEBUG_CONVEX_HULL + b3Printf("\n\nMerge\n"); + result.print(); + hull1.print(); +#endif + merge(result, hull1); +#ifdef DEBUG_CONVEX_HULL + b3Printf("\n Result\n"); + result.print(); +#endif +} + +#ifdef DEBUG_CONVEX_HULL +void b3ConvexHullInternal::IntermediateHull::print() +{ + b3Printf(" Hull\n"); + for (Vertex* v = minXy; v; ) + { + b3Printf(" "); + v->print(); + if (v == maxXy) + { + b3Printf(" maxXy"); + } + if (v == minYx) + { + b3Printf(" minYx"); + } + if (v == maxYx) + { + b3Printf(" maxYx"); + } + if (v->next->prev != v) + { + b3Printf(" Inconsistency"); + } + b3Printf("\n"); + v = v->next; + if (v == minXy) + { + break; + } + } + if (minXy) + { + minXy->copy = (minXy->copy == -1) ? -2 : -1; + minXy->printGraph(); + } +} + +void b3ConvexHullInternal::Vertex::printGraph() +{ + print(); + b3Printf("\nEdges\n"); + Edge* e = edges; + if (e) + { + do + { + e->print(); + b3Printf("\n"); + e = e->next; + } while (e != edges); + do + { + Vertex* v = e->target; + if (v->copy != copy) + { + v->copy = copy; + v->printGraph(); + } + e = e->next; + } while (e != edges); + } +} +#endif + +b3ConvexHullInternal::Orientation b3ConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t) +{ + b3Assert(prev->reverse->target == next->reverse->target); + if (prev->next == next) + { + if (prev->prev == next) + { + Point64 n = t.cross(s); + Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target); + b3Assert(!m.isZero()); + btInt64_t dot = n.dot(m); + b3Assert(dot != 0); + return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE; + } + return COUNTER_CLOCKWISE; + } + else if (prev->prev == next) + { + return CLOCKWISE; + } + else + { + return NONE; + } +} + +b3ConvexHullInternal::Edge* b3ConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot) +{ + Edge* minEdge = NULL; + +#ifdef DEBUG_CONVEX_HULL + b3Printf("find max edge for %d\n", start->point.index); +#endif + Edge* e = start->edges; + if (e) + { + do + { + if (e->copy > mergeStamp) + { + Point32 t = *e->target - *start; + Rational64 cot(t.dot(sxrxs), t.dot(rxs)); +#ifdef DEBUG_CONVEX_HULL + b3Printf(" Angle is %f (%d) for ", (float) b3Atan(cot.toScalar()), (int) cot.isNaN()); + e->print(); +#endif + if (cot.isNaN()) + { + b3Assert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0)); + } + else + { + int cmp; + if (minEdge == NULL) + { + minCot = cot; + minEdge = e; + } + else if ((cmp = cot.compare(minCot)) < 0) + { + minCot = cot; + minEdge = e; + } + else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE))) + { + minEdge = e; + } + } +#ifdef DEBUG_CONVEX_HULL + b3Printf("\n"); +#endif + } + e = e->next; + } while (e != start->edges); + } + return minEdge; +} + +void b3ConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1) +{ + Edge* start0 = e0; + Edge* start1 = e1; + Point32 et0 = start0 ? start0->target->point : c0->point; + Point32 et1 = start1 ? start1->target->point : c1->point; + Point32 s = c1->point - c0->point; + Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s); + btInt64_t dist = c0->point.dot(normal); + b3Assert(!start1 || (start1->target->point.dot(normal) == dist)); + Point64 perp = s.cross(normal); + b3Assert(!perp.isZero()); + +#ifdef DEBUG_CONVEX_HULL + b3Printf(" Advancing %d %d (%p %p, %d %d)\n", c0->point.index, c1->point.index, start0, start1, start0 ? start0->target->point.index : -1, start1 ? start1->target->point.index : -1); +#endif + + btInt64_t maxDot0 = et0.dot(perp); + if (e0) + { + while (e0->target != stop0) + { + Edge* e = e0->reverse->prev; + if (e->target->point.dot(normal) < dist) + { + break; + } + b3Assert(e->target->point.dot(normal) == dist); + if (e->copy == mergeStamp) + { + break; + } + btInt64_t dot = e->target->point.dot(perp); + if (dot <= maxDot0) + { + break; + } + maxDot0 = dot; + e0 = e; + et0 = e->target->point; + } + } + + btInt64_t maxDot1 = et1.dot(perp); + if (e1) + { + while (e1->target != stop1) + { + Edge* e = e1->reverse->next; + if (e->target->point.dot(normal) < dist) + { + break; + } + b3Assert(e->target->point.dot(normal) == dist); + if (e->copy == mergeStamp) + { + break; + } + btInt64_t dot = e->target->point.dot(perp); + if (dot <= maxDot1) + { + break; + } + maxDot1 = dot; + e1 = e; + et1 = e->target->point; + } + } + +#ifdef DEBUG_CONVEX_HULL + b3Printf(" Starting at %d %d\n", et0.index, et1.index); +#endif + + btInt64_t dx = maxDot1 - maxDot0; + if (dx > 0) + { + while (true) + { + btInt64_t dy = (et1 - et0).dot(s); + + if (e0 && (e0->target != stop0)) + { + Edge* f0 = e0->next->reverse; + if (f0->copy > mergeStamp) + { + btInt64_t dx0 = (f0->target->point - et0).dot(perp); + btInt64_t dy0 = (f0->target->point - et0).dot(s); + if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0))) + { + et0 = f0->target->point; + dx = (et1 - et0).dot(perp); + e0 = (e0 == start0) ? NULL : f0; + continue; + } + } + } + + if (e1 && (e1->target != stop1)) + { + Edge* f1 = e1->reverse->next; + if (f1->copy > mergeStamp) + { + Point32 d1 = f1->target->point - et1; + if (d1.dot(normal) == 0) + { + btInt64_t dx1 = d1.dot(perp); + btInt64_t dy1 = d1.dot(s); + btInt64_t dxn = (f1->target->point - et0).dot(perp); + if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0)))) + { + e1 = f1; + et1 = e1->target->point; + dx = dxn; + continue; + } + } + else + { + b3Assert((e1 == start1) && (d1.dot(normal) < 0)); + } + } + } + + break; + } + } + else if (dx < 0) + { + while (true) + { + btInt64_t dy = (et1 - et0).dot(s); + + if (e1 && (e1->target != stop1)) + { + Edge* f1 = e1->prev->reverse; + if (f1->copy > mergeStamp) + { + btInt64_t dx1 = (f1->target->point - et1).dot(perp); + btInt64_t dy1 = (f1->target->point - et1).dot(s); + if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0))) + { + et1 = f1->target->point; + dx = (et1 - et0).dot(perp); + e1 = (e1 == start1) ? NULL : f1; + continue; + } + } + } + + if (e0 && (e0->target != stop0)) + { + Edge* f0 = e0->reverse->prev; + if (f0->copy > mergeStamp) + { + Point32 d0 = f0->target->point - et0; + if (d0.dot(normal) == 0) + { + btInt64_t dx0 = d0.dot(perp); + btInt64_t dy0 = d0.dot(s); + btInt64_t dxn = (et1 - f0->target->point).dot(perp); + if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0)))) + { + e0 = f0; + et0 = e0->target->point; + dx = dxn; + continue; + } + } + else + { + b3Assert((e0 == start0) && (d0.dot(normal) < 0)); + } + } + } + + break; + } + } +#ifdef DEBUG_CONVEX_HULL + b3Printf(" Advanced edges to %d %d\n", et0.index, et1.index); +#endif +} + + +void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) +{ + if (!h1.maxXy) + { + return; + } + if (!h0.maxXy) + { + h0 = h1; + return; + } + + mergeStamp--; + + Vertex* c0 = NULL; + Edge* toPrev0 = NULL; + Edge* firstNew0 = NULL; + Edge* pendingHead0 = NULL; + Edge* pendingTail0 = NULL; + Vertex* c1 = NULL; + Edge* toPrev1 = NULL; + Edge* firstNew1 = NULL; + Edge* pendingHead1 = NULL; + Edge* pendingTail1 = NULL; + Point32 prevPoint; + + if (mergeProjection(h0, h1, c0, c1)) + { + Point32 s = *c1 - *c0; + Point64 normal = Point32(0, 0, -1).cross(s); + Point64 t = s.cross(normal); + b3Assert(!t.isZero()); + + Edge* e = c0->edges; + Edge* start0 = NULL; + if (e) + { + do + { + btInt64_t dot = (*e->target - *c0).dot(normal); + b3Assert(dot <= 0); + if ((dot == 0) && ((*e->target - *c0).dot(t) > 0)) + { + if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE)) + { + start0 = e; + } + } + e = e->next; + } while (e != c0->edges); + } + + e = c1->edges; + Edge* start1 = NULL; + if (e) + { + do + { + btInt64_t dot = (*e->target - *c1).dot(normal); + b3Assert(dot <= 0); + if ((dot == 0) && ((*e->target - *c1).dot(t) > 0)) + { + if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE)) + { + start1 = e; + } + } + e = e->next; + } while (e != c1->edges); + } + + if (start0 || start1) + { + findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL); + if (start0) + { + c0 = start0->target; + } + if (start1) + { + c1 = start1->target; + } + } + + prevPoint = c1->point; + prevPoint.z++; + } + else + { + prevPoint = c1->point; + prevPoint.x++; + } + + Vertex* first0 = c0; + Vertex* first1 = c1; + bool firstRun = true; + + while (true) + { + Point32 s = *c1 - *c0; + Point32 r = prevPoint - c0->point; + Point64 rxs = r.cross(s); + Point64 sxrxs = s.cross(rxs); + +#ifdef DEBUG_CONVEX_HULL + b3Printf("\n Checking %d %d\n", c0->point.index, c1->point.index); +#endif + Rational64 minCot0(0, 0); + Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0); + Rational64 minCot1(0, 0); + Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1); + if (!min0 && !min1) + { + Edge* e = newEdgePair(c0, c1); + e->link(e); + c0->edges = e; + + e = e->reverse; + e->link(e); + c1->edges = e; + return; + } + else + { + int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1); +#ifdef DEBUG_CONVEX_HULL + b3Printf(" -> Result %d\n", cmp); +#endif + if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity())) + { + Edge* e = newEdgePair(c0, c1); + if (pendingTail0) + { + pendingTail0->prev = e; + } + else + { + pendingHead0 = e; + } + e->next = pendingTail0; + pendingTail0 = e; + + e = e->reverse; + if (pendingTail1) + { + pendingTail1->next = e; + } + else + { + pendingHead1 = e; + } + e->prev = pendingTail1; + pendingTail1 = e; + } + + Edge* e0 = min0; + Edge* e1 = min1; + +#ifdef DEBUG_CONVEX_HULL + b3Printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1); +#endif + + if (cmp == 0) + { + findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL); + } + + if ((cmp >= 0) && e1) + { + if (toPrev1) + { + for (Edge* e = toPrev1->next, *n = NULL; e != min1; e = n) + { + n = e->next; + removeEdgePair(e); + } + } + + if (pendingTail1) + { + if (toPrev1) + { + toPrev1->link(pendingHead1); + } + else + { + min1->prev->link(pendingHead1); + firstNew1 = pendingHead1; + } + pendingTail1->link(min1); + pendingHead1 = NULL; + pendingTail1 = NULL; + } + else if (!toPrev1) + { + firstNew1 = min1; + } + + prevPoint = c1->point; + c1 = e1->target; + toPrev1 = e1->reverse; + } + + if ((cmp <= 0) && e0) + { + if (toPrev0) + { + for (Edge* e = toPrev0->prev, *n = NULL; e != min0; e = n) + { + n = e->prev; + removeEdgePair(e); + } + } + + if (pendingTail0) + { + if (toPrev0) + { + pendingHead0->link(toPrev0); + } + else + { + pendingHead0->link(min0->next); + firstNew0 = pendingHead0; + } + min0->link(pendingTail0); + pendingHead0 = NULL; + pendingTail0 = NULL; + } + else if (!toPrev0) + { + firstNew0 = min0; + } + + prevPoint = c0->point; + c0 = e0->target; + toPrev0 = e0->reverse; + } + } + + if ((c0 == first0) && (c1 == first1)) + { + if (toPrev0 == NULL) + { + pendingHead0->link(pendingTail0); + c0->edges = pendingTail0; + } + else + { + for (Edge* e = toPrev0->prev, *n = NULL; e != firstNew0; e = n) + { + n = e->prev; + removeEdgePair(e); + } + if (pendingTail0) + { + pendingHead0->link(toPrev0); + firstNew0->link(pendingTail0); + } + } + + if (toPrev1 == NULL) + { + pendingTail1->link(pendingHead1); + c1->edges = pendingTail1; + } + else + { + for (Edge* e = toPrev1->next, *n = NULL; e != firstNew1; e = n) + { + n = e->next; + removeEdgePair(e); + } + if (pendingTail1) + { + toPrev1->link(pendingHead1); + pendingTail1->link(firstNew1); + } + } + + return; + } + + firstRun = false; + } +} + + +static bool b3PointCmp(const b3ConvexHullInternal::Point32& p, const b3ConvexHullInternal::Point32& q) +{ + return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z)))); +} + +void b3ConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count) +{ + b3Vector3 min = b3MakeVector3(b3Scalar(1e30), b3Scalar(1e30), b3Scalar(1e30)), max = b3MakeVector3(b3Scalar(-1e30), b3Scalar(-1e30), b3Scalar(-1e30)); + const char* ptr = (const char*) coords; + if (doubleCoords) + { + for (int i = 0; i < count; i++) + { + const double* v = (const double*) ptr; + b3Vector3 p = b3MakeVector3((b3Scalar) v[0], (b3Scalar) v[1], (b3Scalar) v[2]); + ptr += stride; + min.setMin(p); + max.setMax(p); + } + } + else + { + for (int i = 0; i < count; i++) + { + const float* v = (const float*) ptr; + b3Vector3 p = b3MakeVector3(v[0], v[1], v[2]); + ptr += stride; + min.setMin(p); + max.setMax(p); + } + } + + b3Vector3 s = max - min; + maxAxis = s.maxAxis(); + minAxis = s.minAxis(); + if (minAxis == maxAxis) + { + minAxis = (maxAxis + 1) % 3; + } + medAxis = 3 - maxAxis - minAxis; + + s /= b3Scalar(10216); + if (((medAxis + 1) % 3) != maxAxis) + { + s *= -1; + } + scaling = s; + + if (s[0] != 0) + { + s[0] = b3Scalar(1) / s[0]; + } + if (s[1] != 0) + { + s[1] = b3Scalar(1) / s[1]; + } + if (s[2] != 0) + { + s[2] = b3Scalar(1) / s[2]; + } + + center = (min + max) * b3Scalar(0.5); + + b3AlignedObjectArray points; + points.resize(count); + ptr = (const char*) coords; + if (doubleCoords) + { + for (int i = 0; i < count; i++) + { + const double* v = (const double*) ptr; + b3Vector3 p = b3MakeVector3((b3Scalar) v[0], (b3Scalar) v[1], (b3Scalar) v[2]); + ptr += stride; + p = (p - center) * s; + points[i].x = (btInt32_t) p[medAxis]; + points[i].y = (btInt32_t) p[maxAxis]; + points[i].z = (btInt32_t) p[minAxis]; + points[i].index = i; + } + } + else + { + for (int i = 0; i < count; i++) + { + const float* v = (const float*) ptr; + b3Vector3 p = b3MakeVector3(v[0], v[1], v[2]); + ptr += stride; + p = (p - center) * s; + points[i].x = (btInt32_t) p[medAxis]; + points[i].y = (btInt32_t) p[maxAxis]; + points[i].z = (btInt32_t) p[minAxis]; + points[i].index = i; + } + } + points.quickSort(b3PointCmp); + + vertexPool.reset(); + vertexPool.setArraySize(count); + originalVertices.resize(count); + for (int i = 0; i < count; i++) + { + Vertex* v = vertexPool.newObject(); + v->edges = NULL; + v->point = points[i]; + v->copy = -1; + originalVertices[i] = v; + } + + points.clear(); + + edgePool.reset(); + edgePool.setArraySize(6 * count); + + usedEdgePairs = 0; + maxUsedEdgePairs = 0; + + mergeStamp = -3; + + IntermediateHull hull; + computeInternal(0, count, hull); + vertexList = hull.minXy; +#ifdef DEBUG_CONVEX_HULL + b3Printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count); +#endif +} + +b3Vector3 b3ConvexHullInternal::toBtVector(const Point32& v) +{ + b3Vector3 p; + p[medAxis] = b3Scalar(v.x); + p[maxAxis] = b3Scalar(v.y); + p[minAxis] = b3Scalar(v.z); + return p * scaling; +} + +b3Vector3 b3ConvexHullInternal::getBtNormal(Face* face) +{ + return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized(); +} + +b3Vector3 b3ConvexHullInternal::getCoordinates(const Vertex* v) +{ + b3Vector3 p; + p[medAxis] = v->xvalue(); + p[maxAxis] = v->yvalue(); + p[minAxis] = v->zvalue(); + return p * scaling + center; +} + +b3Scalar b3ConvexHullInternal::shrink(b3Scalar amount, b3Scalar clampAmount) +{ + if (!vertexList) + { + return 0; + } + int stamp = --mergeStamp; + b3AlignedObjectArray stack; + vertexList->copy = stamp; + stack.push_back(vertexList); + b3AlignedObjectArray faces; + + Point32 ref = vertexList->point; + Int128 hullCenterX(0, 0); + Int128 hullCenterY(0, 0); + Int128 hullCenterZ(0, 0); + Int128 volume(0, 0); + + while (stack.size() > 0) + { + Vertex* v = stack[stack.size() - 1]; + stack.pop_back(); + Edge* e = v->edges; + if (e) + { + do + { + if (e->target->copy != stamp) + { + e->target->copy = stamp; + stack.push_back(e->target); + } + if (e->copy != stamp) + { + Face* face = facePool.newObject(); + face->init(e->target, e->reverse->prev->target, v); + faces.push_back(face); + Edge* f = e; + + Vertex* a = NULL; + Vertex* b = NULL; + do + { + if (a && b) + { + btInt64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref)); + b3Assert(vol >= 0); + Point32 c = v->point + a->point + b->point + ref; + hullCenterX += vol * c.x; + hullCenterY += vol * c.y; + hullCenterZ += vol * c.z; + volume += vol; + } + + b3Assert(f->copy != stamp); + f->copy = stamp; + f->face = face; + + a = b; + b = f->target; + + f = f->reverse->prev; + } while (f != e); + } + e = e->next; + } while (e != v->edges); + } + } + + if (volume.getSign() <= 0) + { + return 0; + } + + b3Vector3 hullCenter; + hullCenter[medAxis] = hullCenterX.toScalar(); + hullCenter[maxAxis] = hullCenterY.toScalar(); + hullCenter[minAxis] = hullCenterZ.toScalar(); + hullCenter /= 4 * volume.toScalar(); + hullCenter *= scaling; + + int faceCount = faces.size(); + + if (clampAmount > 0) + { + b3Scalar minDist = B3_INFINITY; + for (int i = 0; i < faceCount; i++) + { + b3Vector3 normal = getBtNormal(faces[i]); + b3Scalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter); + if (dist < minDist) + { + minDist = dist; + } + } + + if (minDist <= 0) + { + return 0; + } + + amount = b3Min(amount, minDist * clampAmount); + } + + unsigned int seed = 243703; + for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223) + { + b3Swap(faces[i], faces[seed % faceCount]); + } + + for (int i = 0; i < faceCount; i++) + { + if (!shiftFace(faces[i], amount, stack)) + { + return -amount; + } + } + + return amount; +} + +bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjectArray stack) +{ + b3Vector3 origShift = getBtNormal(face) * -amount; + if (scaling[0] != 0) + { + origShift[0] /= scaling[0]; + } + if (scaling[1] != 0) + { + origShift[1] /= scaling[1]; + } + if (scaling[2] != 0) + { + origShift[2] /= scaling[2]; + } + Point32 shift((btInt32_t) origShift[medAxis], (btInt32_t) origShift[maxAxis], (btInt32_t) origShift[minAxis]); + if (shift.isZero()) + { + return true; + } + Point64 normal = face->getNormal(); +#ifdef DEBUG_CONVEX_HULL + b3Printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n", + face->origin.x, face->origin.y, face->origin.z, face->dir0.x, face->dir0.y, face->dir0.z, face->dir1.x, face->dir1.y, face->dir1.z, shift.x, shift.y, shift.z); +#endif + btInt64_t origDot = face->origin.dot(normal); + Point32 shiftedOrigin = face->origin + shift; + btInt64_t shiftedDot = shiftedOrigin.dot(normal); + b3Assert(shiftedDot <= origDot); + if (shiftedDot >= origDot) + { + return false; + } + + Edge* intersection = NULL; + + Edge* startEdge = face->nearbyVertex->edges; +#ifdef DEBUG_CONVEX_HULL + b3Printf("Start edge is "); + startEdge->print(); + b3Printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot); +#endif + Rational128 optDot = face->nearbyVertex->dot(normal); + int cmp = optDot.compare(shiftedDot); +#ifdef SHOW_ITERATIONS + int n = 0; +#endif + if (cmp >= 0) + { + Edge* e = startEdge; + do + { +#ifdef SHOW_ITERATIONS + n++; +#endif + Rational128 dot = e->target->dot(normal); + b3Assert(dot.compare(origDot) <= 0); +#ifdef DEBUG_CONVEX_HULL + b3Printf("Moving downwards, edge is "); + e->print(); + b3Printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot); +#endif + if (dot.compare(optDot) < 0) + { + int c = dot.compare(shiftedDot); + optDot = dot; + e = e->reverse; + startEdge = e; + if (c < 0) + { + intersection = e; + break; + } + cmp = c; + } + e = e->prev; + } while (e != startEdge); + + if (!intersection) + { + return false; + } + } + else + { + Edge* e = startEdge; + do + { +#ifdef SHOW_ITERATIONS + n++; +#endif + Rational128 dot = e->target->dot(normal); + b3Assert(dot.compare(origDot) <= 0); +#ifdef DEBUG_CONVEX_HULL + b3Printf("Moving upwards, edge is "); + e->print(); + b3Printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot); +#endif + if (dot.compare(optDot) > 0) + { + cmp = dot.compare(shiftedDot); + if (cmp >= 0) + { + intersection = e; + break; + } + optDot = dot; + e = e->reverse; + startEdge = e; + } + e = e->prev; + } while (e != startEdge); + + if (!intersection) + { + return true; + } + } + +#ifdef SHOW_ITERATIONS + b3Printf("Needed %d iterations to find initial intersection\n", n); +#endif + + if (cmp == 0) + { + Edge* e = intersection->reverse->next; +#ifdef SHOW_ITERATIONS + n = 0; +#endif + while (e->target->dot(normal).compare(shiftedDot) <= 0) + { +#ifdef SHOW_ITERATIONS + n++; +#endif + e = e->next; + if (e == intersection->reverse) + { + return true; + } +#ifdef DEBUG_CONVEX_HULL + b3Printf("Checking for outwards edge, current edge is "); + e->print(); + b3Printf("\n"); +#endif + } +#ifdef SHOW_ITERATIONS + b3Printf("Needed %d iterations to check for complete containment\n", n); +#endif + } + + Edge* firstIntersection = NULL; + Edge* faceEdge = NULL; + Edge* firstFaceEdge = NULL; + +#ifdef SHOW_ITERATIONS + int m = 0; +#endif + while (true) + { +#ifdef SHOW_ITERATIONS + m++; +#endif +#ifdef DEBUG_CONVEX_HULL + b3Printf("Intersecting edge is "); + intersection->print(); + b3Printf("\n"); +#endif + if (cmp == 0) + { + Edge* e = intersection->reverse->next; + startEdge = e; +#ifdef SHOW_ITERATIONS + n = 0; +#endif + while (true) + { +#ifdef SHOW_ITERATIONS + n++; +#endif + if (e->target->dot(normal).compare(shiftedDot) >= 0) + { + break; + } + intersection = e->reverse; + e = e->next; + if (e == startEdge) + { + return true; + } + } +#ifdef SHOW_ITERATIONS + b3Printf("Needed %d iterations to advance intersection\n", n); +#endif + } + +#ifdef DEBUG_CONVEX_HULL + b3Printf("Advanced intersecting edge to "); + intersection->print(); + b3Printf(", cmp = %d\n", cmp); +#endif + + if (!firstIntersection) + { + firstIntersection = intersection; + } + else if (intersection == firstIntersection) + { + break; + } + + int prevCmp = cmp; + Edge* prevIntersection = intersection; + Edge* prevFaceEdge = faceEdge; + + Edge* e = intersection->reverse; +#ifdef SHOW_ITERATIONS + n = 0; +#endif + while (true) + { +#ifdef SHOW_ITERATIONS + n++; +#endif + e = e->reverse->prev; + b3Assert(e != intersection->reverse); + cmp = e->target->dot(normal).compare(shiftedDot); +#ifdef DEBUG_CONVEX_HULL + b3Printf("Testing edge "); + e->print(); + b3Printf(" -> cmp = %d\n", cmp); +#endif + if (cmp >= 0) + { + intersection = e; + break; + } + } +#ifdef SHOW_ITERATIONS + b3Printf("Needed %d iterations to find other intersection of face\n", n); +#endif + + if (cmp > 0) + { + Vertex* removed = intersection->target; + e = intersection->reverse; + if (e->prev == e) + { + removed->edges = NULL; + } + else + { + removed->edges = e->prev; + e->prev->link(e->next); + e->link(e); + } +#ifdef DEBUG_CONVEX_HULL + b3Printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); +#endif + + Point64 n0 = intersection->face->getNormal(); + Point64 n1 = intersection->reverse->face->getNormal(); + btInt64_t m00 = face->dir0.dot(n0); + btInt64_t m01 = face->dir1.dot(n0); + btInt64_t m10 = face->dir0.dot(n1); + btInt64_t m11 = face->dir1.dot(n1); + btInt64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0); + btInt64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1); + Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10); + b3Assert(det.getSign() != 0); + Vertex* v = vertexPool.newObject(); + v->point.index = -1; + v->copy = -1; + v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01) + + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x, + Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01) + + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y, + Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01) + + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z, + det); + v->point.x = (btInt32_t) v->point128.xvalue(); + v->point.y = (btInt32_t) v->point128.yvalue(); + v->point.z = (btInt32_t) v->point128.zvalue(); + intersection->target = v; + v->edges = e; + + stack.push_back(v); + stack.push_back(removed); + stack.push_back(NULL); + } + + if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target)) + { + faceEdge = newEdgePair(prevIntersection->target, intersection->target); + if (prevCmp == 0) + { + faceEdge->link(prevIntersection->reverse->next); + } + if ((prevCmp == 0) || prevFaceEdge) + { + prevIntersection->reverse->link(faceEdge); + } + if (cmp == 0) + { + intersection->reverse->prev->link(faceEdge->reverse); + } + faceEdge->reverse->link(intersection->reverse); + } + else + { + faceEdge = prevIntersection->reverse->next; + } + + if (prevFaceEdge) + { + if (prevCmp > 0) + { + faceEdge->link(prevFaceEdge->reverse); + } + else if (faceEdge != prevFaceEdge->reverse) + { + stack.push_back(prevFaceEdge->target); + while (faceEdge->next != prevFaceEdge->reverse) + { + Vertex* removed = faceEdge->next->target; + removeEdgePair(faceEdge->next); + stack.push_back(removed); +#ifdef DEBUG_CONVEX_HULL + b3Printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); +#endif + } + stack.push_back(NULL); + } + } + faceEdge->face = face; + faceEdge->reverse->face = intersection->face; + + if (!firstFaceEdge) + { + firstFaceEdge = faceEdge; + } + } +#ifdef SHOW_ITERATIONS + b3Printf("Needed %d iterations to process all intersections\n", m); +#endif + + if (cmp > 0) + { + firstFaceEdge->reverse->target = faceEdge->target; + firstIntersection->reverse->link(firstFaceEdge); + firstFaceEdge->link(faceEdge->reverse); + } + else if (firstFaceEdge != faceEdge->reverse) + { + stack.push_back(faceEdge->target); + while (firstFaceEdge->next != faceEdge->reverse) + { + Vertex* removed = firstFaceEdge->next->target; + removeEdgePair(firstFaceEdge->next); + stack.push_back(removed); +#ifdef DEBUG_CONVEX_HULL + b3Printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); +#endif + } + stack.push_back(NULL); + } + + b3Assert(stack.size() > 0); + vertexList = stack[0]; + +#ifdef DEBUG_CONVEX_HULL + b3Printf("Removing part\n"); +#endif +#ifdef SHOW_ITERATIONS + n = 0; +#endif + int pos = 0; + while (pos < stack.size()) + { + int end = stack.size(); + while (pos < end) + { + Vertex* kept = stack[pos++]; +#ifdef DEBUG_CONVEX_HULL + kept->print(); +#endif + bool deeper = false; + Vertex* removed; + while ((removed = stack[pos++]) != NULL) + { +#ifdef SHOW_ITERATIONS + n++; +#endif + kept->receiveNearbyFaces(removed); + while (removed->edges) + { + if (!deeper) + { + deeper = true; + stack.push_back(kept); + } + stack.push_back(removed->edges->target); + removeEdgePair(removed->edges); + } + } + if (deeper) + { + stack.push_back(NULL); + } + } + } +#ifdef SHOW_ITERATIONS + b3Printf("Needed %d iterations to remove part\n", n); +#endif + + stack.resize(0); + face->origin = shiftedOrigin; + + return true; +} + + +static int getVertexCopy(b3ConvexHullInternal::Vertex* vertex, b3AlignedObjectArray& vertices) +{ + int index = vertex->copy; + if (index < 0) + { + index = vertices.size(); + vertex->copy = index; + vertices.push_back(vertex); +#ifdef DEBUG_CONVEX_HULL + b3Printf("Vertex %d gets index *%d\n", vertex->point.index, index); +#endif + } + return index; +} + +b3Scalar b3ConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp) +{ + if (count <= 0) + { + vertices.clear(); + edges.clear(); + faces.clear(); + return 0; + } + + b3ConvexHullInternal hull; + hull.compute(coords, doubleCoords, stride, count); + + b3Scalar shift = 0; + if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0)) + { + vertices.clear(); + edges.clear(); + faces.clear(); + return shift; + } + + vertices.resize(0); + edges.resize(0); + faces.resize(0); + + b3AlignedObjectArray oldVertices; + getVertexCopy(hull.vertexList, oldVertices); + int copied = 0; + while (copied < oldVertices.size()) + { + b3ConvexHullInternal::Vertex* v = oldVertices[copied]; + vertices.push_back(hull.getCoordinates(v)); + b3ConvexHullInternal::Edge* firstEdge = v->edges; + if (firstEdge) + { + int firstCopy = -1; + int prevCopy = -1; + b3ConvexHullInternal::Edge* e = firstEdge; + do + { + if (e->copy < 0) + { + int s = edges.size(); + edges.push_back(Edge()); + edges.push_back(Edge()); + Edge* c = &edges[s]; + Edge* r = &edges[s + 1]; + e->copy = s; + e->reverse->copy = s + 1; + c->reverse = 1; + r->reverse = -1; + c->targetVertex = getVertexCopy(e->target, oldVertices); + r->targetVertex = copied; +#ifdef DEBUG_CONVEX_HULL + b3Printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex()); +#endif + } + if (prevCopy >= 0) + { + edges[e->copy].next = prevCopy - e->copy; + } + else + { + firstCopy = e->copy; + } + prevCopy = e->copy; + e = e->next; + } while (e != firstEdge); + edges[firstCopy].next = prevCopy - firstCopy; + } + copied++; + } + + for (int i = 0; i < copied; i++) + { + b3ConvexHullInternal::Vertex* v = oldVertices[i]; + b3ConvexHullInternal::Edge* firstEdge = v->edges; + if (firstEdge) + { + b3ConvexHullInternal::Edge* e = firstEdge; + do + { + if (e->copy >= 0) + { +#ifdef DEBUG_CONVEX_HULL + b3Printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex()); +#endif + faces.push_back(e->copy); + b3ConvexHullInternal::Edge* f = e; + do + { +#ifdef DEBUG_CONVEX_HULL + b3Printf(" Face *%d\n", edges[f->copy].getTargetVertex()); +#endif + f->copy = -1; + f = f->reverse->prev; + } while (f != e); + } + e = e->next; + } while (e != firstEdge); + } + } + + return shift; +} + + + + + diff --git a/extern/bullet/src/Bullet3Geometry/b3ConvexHullComputer.h b/extern/bullet/src/Bullet3Geometry/b3ConvexHullComputer.h new file mode 100644 index 000000000000..6dcc931a78cf --- /dev/null +++ b/extern/bullet/src/Bullet3Geometry/b3ConvexHullComputer.h @@ -0,0 +1,103 @@ +/* +Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net + +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 B3_CONVEX_HULL_COMPUTER_H +#define B3_CONVEX_HULL_COMPUTER_H + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +/// Convex hull implementation based on Preparata and Hong +/// See http://code.google.com/p/bullet/issues/detail?id=275 +/// Ole Kniemeyer, MAXON Computer GmbH +class b3ConvexHullComputer +{ + private: + b3Scalar compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp); + + public: + + class Edge + { + private: + int next; + int reverse; + int targetVertex; + + friend class b3ConvexHullComputer; + + public: + int getSourceVertex() const + { + return (this + reverse)->targetVertex; + } + + int getTargetVertex() const + { + return targetVertex; + } + + const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex + { + return this + next; + } + + const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face + { + return (this + reverse)->getNextEdgeOfVertex(); + } + + const Edge* getReverseEdge() const + { + return this + reverse; + } + }; + + + // Vertices of the output hull + b3AlignedObjectArray vertices; + + // Edges of the output hull + b3AlignedObjectArray edges; + + // Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons + b3AlignedObjectArray faces; + + /* + Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes + between the addresses of consecutive vertices. If "shrink" is positive, the convex hull is shrunken + by that amount (each face is moved by "shrink" length units towards the center along its normal). + If "shrinkClamp" is positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where "innerRadius" + is the minimum distance of a face to the center of the convex hull. + + The returned value is the amount by which the hull has been shrunken. If it is negative, the amount was so large + that the resulting convex hull is empty. + + The output convex hull can be found in the member variables "vertices", "edges", "faces". + */ + b3Scalar compute(const float* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp) + { + return compute(coords, false, stride, count, shrink, shrinkClamp); + } + + // same as above, but double precision + b3Scalar compute(const double* coords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp) + { + return compute(coords, true, stride, count, shrink, shrinkClamp); + } +}; + + +#endif //B3_CONVEX_HULL_COMPUTER_H + diff --git a/extern/bullet/src/Bullet3Geometry/b3GeometryUtil.cpp b/extern/bullet/src/Bullet3Geometry/b3GeometryUtil.cpp new file mode 100644 index 000000000000..dd80fed6bd95 --- /dev/null +++ b/extern/bullet/src/Bullet3Geometry/b3GeometryUtil.cpp @@ -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 "b3GeometryUtil.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 b3BulletMathProbe (); + + void b3BulletMathProbe () {} +} + + +bool b3GeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray& planeEquations, const b3Vector3& point, b3Scalar margin) +{ + int numbrushes = planeEquations.size(); + for (int i=0;ib3Scalar(0.)) + { + return false; + } + } + return true; + +} + + +bool b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray& vertices, b3Scalar margin) +{ + int numvertices = vertices.size(); + for (int i=0;ib3Scalar(0.)) + { + return false; + } + } + return true; +} + +bool notExist(const b3Vector3& planeEquation,const b3AlignedObjectArray& planeEquations); + +bool notExist(const b3Vector3& planeEquation,const b3AlignedObjectArray& planeEquations) +{ + int numbrushes = planeEquations.size(); + for (int i=0;i b3Scalar(0.999)) + { + return false; + } + } + return true; +} + +void b3GeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray& vertices, b3AlignedObjectArray& planeEquationsOut ) +{ + const int numvertices = vertices.size(); + // brute force: + for (int i=0;i b3Scalar(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,b3Scalar(0.01))) + { + planeEquationsOut.push_back(planeEquation); + } + } + } + normalSign = b3Scalar(-1.); + } + + } + } + } + +} + +void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray& planeEquations , b3AlignedObjectArray& verticesOut ) +{ + const int numbrushes = planeEquations.size(); + // brute force: + for (int i=0;i b3Scalar(0.0001) ) && + ( n3n1.length2() > b3Scalar(0.0001) ) && + ( n1n2.length2() > b3Scalar(0.0001) ) ) + { + //point P out of 3 plane equations: + + // d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 ) + //P = ------------------------------------------------------------------------- + // N1 . ( N2 * N3 ) + + + b3Scalar quotient = (N1.dot(n2n3)); + if (b3Fabs(quotient) > b3Scalar(0.000001)) + { + quotient = b3Scalar(-1.) / quotient; + n2n3 *= N1[3]; + n3n1 *= N2[3]; + n1n2 *= N3[3]; + b3Vector3 potentialVertex = n2n3; + potentialVertex += n3n1; + potentialVertex += n1n2; + potentialVertex *= quotient; + + //check if inside, and replace supportingVertexOut if needed + if (isPointInsidePlanes(planeEquations,potentialVertex,b3Scalar(0.01))) + { + verticesOut.push_back(potentialVertex); + } + } + } + } + } + } +} + diff --git a/extern/bullet/src/Bullet3Geometry/b3GeometryUtil.h b/extern/bullet/src/Bullet3Geometry/b3GeometryUtil.h new file mode 100644 index 000000000000..8b5fd7ad62e0 --- /dev/null +++ b/extern/bullet/src/Bullet3Geometry/b3GeometryUtil.h @@ -0,0 +1,42 @@ +/* +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. +*/ + + +#ifndef B3_GEOMETRY_UTIL_H +#define B3_GEOMETRY_UTIL_H + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +///The b3GeometryUtil helper class provides a few methods to convert between plane equations and vertices. +class b3GeometryUtil +{ + public: + + + static void getPlaneEquationsFromVertices(b3AlignedObjectArray& vertices, b3AlignedObjectArray& planeEquationsOut ); + + static void getVerticesFromPlaneEquations(const b3AlignedObjectArray& planeEquations , b3AlignedObjectArray& verticesOut ); + + static bool isInside(const b3AlignedObjectArray& vertices, const b3Vector3& planeNormal, b3Scalar margin); + + static bool isPointInsidePlanes(const b3AlignedObjectArray& planeEquations, const b3Vector3& point, b3Scalar margin); + + static bool areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray& vertices, b3Scalar margin); + +}; + + +#endif //B3_GEOMETRY_UTIL_H + diff --git a/extern/bullet/src/Bullet3Geometry/b3GrahamScan2dConvexHull.h b/extern/bullet/src/Bullet3Geometry/b3GrahamScan2dConvexHull.h new file mode 100644 index 000000000000..1b933c526452 --- /dev/null +++ b/extern/bullet/src/Bullet3Geometry/b3GrahamScan2dConvexHull.h @@ -0,0 +1,117 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011 Advanced Micro Devices, Inc. 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. +*/ + + +#ifndef B3_GRAHAM_SCAN_2D_CONVEX_HULL_H +#define B3_GRAHAM_SCAN_2D_CONVEX_HULL_H + + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +struct b3GrahamVector3 : public b3Vector3 +{ + b3GrahamVector3(const b3Vector3& org, int orgIndex) + :b3Vector3(org), + m_orgIndex(orgIndex) + { + } + b3Scalar m_angle; + int m_orgIndex; +}; + + +struct b3AngleCompareFunc { + b3Vector3 m_anchor; + b3AngleCompareFunc(const b3Vector3& anchor) + : m_anchor(anchor) + { + } + bool operator()(const b3GrahamVector3& a, const b3GrahamVector3& b) const { + if (a.m_angle != b.m_angle) + return a.m_angle < b.m_angle; + else + { + b3Scalar al = (a-m_anchor).length2(); + b3Scalar bl = (b-m_anchor).length2(); + if (al != bl) + return al < bl; + else + { + return a.m_orgIndex < b.m_orgIndex; + } + } + } +}; + +inline void b3GrahamScanConvexHull2D(b3AlignedObjectArray& originalPoints, b3AlignedObjectArray& hull, const b3Vector3& normalAxis) +{ + b3Vector3 axis0,axis1; + b3PlaneSpace1(normalAxis,axis0,axis1); + + + if (originalPoints.size()<=1) + { + for (int i=0;i1) { + b3Vector3& a = hull[hull.size()-2]; + b3Vector3& b = hull[hull.size()-1]; + isConvex = b3Cross(a-b,a-originalPoints[i]).dot(normalAxis)> 0; + if (!isConvex) + hull.pop_back(); + else + hull.push_back(originalPoints[i]); + } + } +} + +#endif //B3_GRAHAM_SCAN_2D_CONVEX_HULL_H diff --git a/extern/bullet/src/Bullet3Geometry/premake4.lua b/extern/bullet/src/Bullet3Geometry/premake4.lua new file mode 100644 index 000000000000..cce93f7ee1d9 --- /dev/null +++ b/extern/bullet/src/Bullet3Geometry/premake4.lua @@ -0,0 +1,16 @@ + project "Bullet3Geometry" + + language "C++" + + kind "StaticLib" + + includedirs {".."} + + if os.is("Linux") then + buildoptions{"-fPIC"} + end + + files { + "**.cpp", + "**.h" + } \ No newline at end of file diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h new file mode 100644 index 000000000000..0ed8aa823265 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h @@ -0,0 +1,44 @@ + +#ifndef B3_GPU_BROADPHASE_INTERFACE_H +#define B3_GPU_BROADPHASE_INTERFACE_H + +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "Bullet3Common/b3Vector3.h" +#include "b3SapAabb.h" +#include "Bullet3Common/shared/b3Int2.h" +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" + +class b3GpuBroadphaseInterface +{ +public: + + typedef class b3GpuBroadphaseInterface* (CreateFunc)(cl_context ctx,cl_device_id device, cl_command_queue q); + + virtual ~b3GpuBroadphaseInterface() + { + } + + virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask)=0; + virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask)=0; + + virtual void calculateOverlappingPairs(int maxPairs)=0; + virtual void calculateOverlappingPairsHost(int maxPairs)=0; + + //call writeAabbsToGpu after done making all changes (createProxy etc) + virtual void writeAabbsToGpu()=0; + + virtual cl_mem getAabbBufferWS()=0; + virtual int getNumOverlap()=0; + virtual cl_mem getOverlappingPairBuffer()=0; + + virtual b3OpenCLArray& getAllAabbsGPU()=0; + virtual b3AlignedObjectArray& getAllAabbsCPU()=0; + + virtual b3OpenCLArray& getOverlappingPairsGPU() = 0; + virtual b3OpenCLArray& getSmallAabbIndicesGPU() = 0; + virtual b3OpenCLArray& getLargeAabbIndicesGPU() = 0; + +}; + +#endif //B3_GPU_BROADPHASE_INTERFACE_H diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp new file mode 100644 index 000000000000..74d0c8056cc6 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp @@ -0,0 +1,385 @@ + +#include "b3GpuGridBroadphase.h" +#include "Bullet3Geometry/b3AabbUtil.h" +#include "kernels/gridBroadphaseKernels.h" +#include "kernels/sapKernels.h" +//#include "kernels/gridBroadphase.cl" + + +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" + + + +#define B3_BROADPHASE_SAP_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl" +#define B3_GRID_BROADPHASE_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphase.cl" + +cl_kernel kCalcHashAABB; +cl_kernel kClearCellStart; +cl_kernel kFindCellStart; +cl_kernel kFindOverlappingPairs; +cl_kernel m_copyAabbsKernel; +cl_kernel m_sap2Kernel; + + + + + +//int maxPairsPerBody = 64; +int maxBodiesPerCell = 256;//?? + +b3GpuGridBroadphase::b3GpuGridBroadphase(cl_context ctx,cl_device_id device, cl_command_queue q ) +:m_context(ctx), +m_device(device), +m_queue(q), +m_allAabbsGPU1(ctx,q), +m_smallAabbsMappingGPU(ctx,q), +m_largeAabbsMappingGPU(ctx,q), +m_gpuPairs(ctx,q), + +m_hashGpu(ctx,q), + +m_cellStartGpu(ctx,q), +m_paramsGPU(ctx,q) +{ + + + b3Vector3 gridSize = b3MakeVector3(3,3,3); + b3Vector3 invGridSize = b3MakeVector3(1.f/gridSize[0],1.f/gridSize[1],1.f/gridSize[2]); + + m_paramsCPU.m_gridSize[0] = 128; + m_paramsCPU.m_gridSize[1] = 128; + m_paramsCPU.m_gridSize[2] = 128; + m_paramsCPU.m_gridSize[3] = maxBodiesPerCell; + m_paramsCPU.setMaxBodiesPerCell(maxBodiesPerCell); + m_paramsCPU.m_invCellSize[0] = invGridSize[0]; + m_paramsCPU.m_invCellSize[1] = invGridSize[1]; + m_paramsCPU.m_invCellSize[2] = invGridSize[2]; + m_paramsCPU.m_invCellSize[3] = 0.f; + m_paramsGPU.push_back(m_paramsCPU); + + cl_int errNum=0; + + { + const char* sapSrc = sapCL; + cl_program sapProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,sapSrc,&errNum,"",B3_BROADPHASE_SAP_PATH); + b3Assert(errNum==CL_SUCCESS); + m_copyAabbsKernel= b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "copyAabbsKernel",&errNum,sapProg ); + m_sap2Kernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "computePairsKernelTwoArrays",&errNum,sapProg ); + b3Assert(errNum==CL_SUCCESS); + } + + { + + cl_program gridProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,gridBroadphaseCL,&errNum,"",B3_GRID_BROADPHASE_PATH); + b3Assert(errNum==CL_SUCCESS); + + kCalcHashAABB = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,gridBroadphaseCL, "kCalcHashAABB",&errNum,gridProg); + b3Assert(errNum==CL_SUCCESS); + + kClearCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,gridBroadphaseCL, "kClearCellStart",&errNum,gridProg); + b3Assert(errNum==CL_SUCCESS); + + kFindCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,gridBroadphaseCL, "kFindCellStart",&errNum,gridProg); + b3Assert(errNum==CL_SUCCESS); + + + kFindOverlappingPairs = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,gridBroadphaseCL, "kFindOverlappingPairs",&errNum,gridProg); + b3Assert(errNum==CL_SUCCESS); + + + + + } + + m_sorter = new b3RadixSort32CL(m_context,m_device,m_queue); + +} +b3GpuGridBroadphase::~b3GpuGridBroadphase() +{ + clReleaseKernel( kCalcHashAABB); + clReleaseKernel( kClearCellStart); + clReleaseKernel( kFindCellStart); + clReleaseKernel( kFindOverlappingPairs); + clReleaseKernel( m_sap2Kernel); + clReleaseKernel( m_copyAabbsKernel); + + + + delete m_sorter; +} + + + +void b3GpuGridBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask) +{ + b3SapAabb aabb; + aabb.m_minVec = aabbMin; + aabb.m_maxVec = aabbMax; + aabb.m_minIndices[3] = userPtr; + aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size();//NOT userPtr; + m_smallAabbsMappingCPU.push_back(m_allAabbsCPU1.size()); + + m_allAabbsCPU1.push_back(aabb); + +} +void b3GpuGridBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask) +{ + b3SapAabb aabb; + aabb.m_minVec = aabbMin; + aabb.m_maxVec = aabbMax; + aabb.m_minIndices[3] = userPtr; + aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size();//NOT userPtr; + m_largeAabbsMappingCPU.push_back(m_allAabbsCPU1.size()); + + m_allAabbsCPU1.push_back(aabb); +} + +void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs) +{ + B3_PROFILE("b3GpuGridBroadphase::calculateOverlappingPairs"); + + + if (0) + { + calculateOverlappingPairsHost(maxPairs); + /* + b3AlignedObjectArray cpuPairs; + m_gpuPairs.copyToHost(cpuPairs); + printf("host m_gpuPairs.size()=%d\n",m_gpuPairs.size()); + for (int i=0;i pairCount(m_context,m_queue); + pairCount.push_back(0); + m_gpuPairs.resize(maxPairs);//numSmallAabbs*maxPairsPerBody); + + { + int numLargeAabbs = m_largeAabbsMappingGPU.size(); + if (numLargeAabbs && numSmallAabbs) + { + B3_PROFILE("sap2Kernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( m_allAabbsGPU1.getBufferCL() ), + b3BufferInfoCL( m_largeAabbsMappingGPU.getBufferCL() ), + b3BufferInfoCL( m_smallAabbsMappingGPU.getBufferCL() ), + b3BufferInfoCL( m_gpuPairs.getBufferCL() ), + b3BufferInfoCL(pairCount.getBufferCL())}; + b3LauncherCL launcher(m_queue, m_sap2Kernel,"m_sap2Kernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( numLargeAabbs ); + launcher.setConst( numSmallAabbs); + launcher.setConst( 0 );//axis is not used + launcher.setConst( maxPairs ); + //@todo: use actual maximum work item sizes of the device instead of hardcoded values + launcher.launch2D( numLargeAabbs, numSmallAabbs,4,64); + + int numPairs = pairCount.at(0); + + if (numPairs >maxPairs) + { + b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs); + numPairs =maxPairs; + } + } + } + + + + + if (numSmallAabbs) + { + B3_PROFILE("gridKernel"); + m_hashGpu.resize(numSmallAabbs); + { + B3_PROFILE("kCalcHashAABB"); + b3LauncherCL launch(m_queue,kCalcHashAABB,"kCalcHashAABB"); + launch.setConst(numSmallAabbs); + launch.setBuffer(m_allAabbsGPU1.getBufferCL()); + launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL()); + launch.setBuffer(m_hashGpu.getBufferCL()); + launch.setBuffer(this->m_paramsGPU.getBufferCL()); + launch.launch1D(numSmallAabbs); + } + + m_sorter->execute(m_hashGpu); + + int numCells = this->m_paramsCPU.m_gridSize[0]*this->m_paramsCPU.m_gridSize[1]*this->m_paramsCPU.m_gridSize[2]; + m_cellStartGpu.resize(numCells); + //b3AlignedObjectArray cellStartCpu; + + + { + B3_PROFILE("kClearCellStart"); + b3LauncherCL launch(m_queue,kClearCellStart,"kClearCellStart"); + launch.setConst(numCells); + launch.setBuffer(m_cellStartGpu.getBufferCL()); + launch.launch1D(numCells); + //m_cellStartGpu.copyToHost(cellStartCpu); + //printf("??\n"); + + } + + + { + B3_PROFILE("kFindCellStart"); + b3LauncherCL launch(m_queue,kFindCellStart,"kFindCellStart"); + launch.setConst(numSmallAabbs); + launch.setBuffer(m_hashGpu.getBufferCL()); + launch.setBuffer(m_cellStartGpu.getBufferCL()); + launch.launch1D(numSmallAabbs); + //m_cellStartGpu.copyToHost(cellStartCpu); + //printf("??\n"); + + } + + { + B3_PROFILE("kFindOverlappingPairs"); + + + b3LauncherCL launch(m_queue,kFindOverlappingPairs,"kFindOverlappingPairs"); + launch.setConst(numSmallAabbs); + launch.setBuffer(m_allAabbsGPU1.getBufferCL()); + launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL()); + launch.setBuffer(m_hashGpu.getBufferCL()); + launch.setBuffer(m_cellStartGpu.getBufferCL()); + + launch.setBuffer(m_paramsGPU.getBufferCL()); + //launch.setBuffer(0); + launch.setBuffer(pairCount.getBufferCL()); + launch.setBuffer(m_gpuPairs.getBufferCL()); + + launch.setConst(maxPairs); + launch.launch1D(numSmallAabbs); + + + int numPairs = pairCount.at(0); + if (numPairs >maxPairs) + { + b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs); + numPairs =maxPairs; + } + + m_gpuPairs.resize(numPairs); + + if (0) + { + b3AlignedObjectArray pairsCpu; + m_gpuPairs.copyToHost(pairsCpu); + + int sz = m_gpuPairs.size(); + printf("m_gpuPairs.size()=%d\n",sz); + for (int i=0;im_allAabbsGPU1.getBufferCL(); +} +int b3GpuGridBroadphase::getNumOverlap() +{ + return m_gpuPairs.size(); +} +cl_mem b3GpuGridBroadphase::getOverlappingPairBuffer() +{ + return m_gpuPairs.getBufferCL(); +} + +b3OpenCLArray& b3GpuGridBroadphase::getAllAabbsGPU() +{ + return m_allAabbsGPU1; +} + +b3AlignedObjectArray& b3GpuGridBroadphase::getAllAabbsCPU() +{ + return m_allAabbsCPU1; +} + +b3OpenCLArray& b3GpuGridBroadphase::getOverlappingPairsGPU() +{ + return m_gpuPairs; +} +b3OpenCLArray& b3GpuGridBroadphase::getSmallAabbIndicesGPU() +{ + return m_smallAabbsMappingGPU; +} +b3OpenCLArray& b3GpuGridBroadphase::getLargeAabbIndicesGPU() +{ + return m_largeAabbsMappingGPU; +} + diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.h b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.h new file mode 100644 index 000000000000..ec18c9f7165a --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.h @@ -0,0 +1,88 @@ +#ifndef B3_GPU_GRID_BROADPHASE_H +#define B3_GPU_GRID_BROADPHASE_H + +#include "b3GpuBroadphaseInterface.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h" + +struct b3ParamsGridBroadphaseCL +{ + + float m_invCellSize[4]; + int m_gridSize[4]; + + int getMaxBodiesPerCell() const + { + return m_gridSize[3]; + } + + void setMaxBodiesPerCell(int maxOverlap) + { + m_gridSize[3] = maxOverlap; + } +}; + + +class b3GpuGridBroadphase : public b3GpuBroadphaseInterface +{ +protected: + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; + + b3OpenCLArray m_allAabbsGPU1; + b3AlignedObjectArray m_allAabbsCPU1; + + b3OpenCLArray m_smallAabbsMappingGPU; + b3AlignedObjectArray m_smallAabbsMappingCPU; + + b3OpenCLArray m_largeAabbsMappingGPU; + b3AlignedObjectArray m_largeAabbsMappingCPU; + + b3AlignedObjectArray m_hostPairs; + b3OpenCLArray m_gpuPairs; + + b3OpenCLArray m_hashGpu; + b3OpenCLArray m_cellStartGpu; + + + b3ParamsGridBroadphaseCL m_paramsCPU; + b3OpenCLArray m_paramsGPU; + + class b3RadixSort32CL* m_sorter; + +public: + + b3GpuGridBroadphase(cl_context ctx,cl_device_id device, cl_command_queue q ); + virtual ~b3GpuGridBroadphase(); + + static b3GpuBroadphaseInterface* CreateFunc(cl_context ctx,cl_device_id device, cl_command_queue q) + { + return new b3GpuGridBroadphase(ctx,device,q); + } + + + + + virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask); + virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask); + + virtual void calculateOverlappingPairs(int maxPairs); + virtual void calculateOverlappingPairsHost(int maxPairs); + + //call writeAabbsToGpu after done making all changes (createProxy etc) + virtual void writeAabbsToGpu(); + + virtual cl_mem getAabbBufferWS(); + virtual int getNumOverlap(); + virtual cl_mem getOverlappingPairBuffer(); + + virtual b3OpenCLArray& getAllAabbsGPU(); + virtual b3AlignedObjectArray& getAllAabbsCPU(); + + virtual b3OpenCLArray& getOverlappingPairsGPU(); + virtual b3OpenCLArray& getSmallAabbIndicesGPU(); + virtual b3OpenCLArray& getLargeAabbIndicesGPU(); + +}; + +#endif //B3_GPU_GRID_BROADPHASE_H \ No newline at end of file diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.cpp b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.cpp new file mode 100644 index 000000000000..641df9eb128e --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.cpp @@ -0,0 +1,577 @@ +/* +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. +*/ +//Initial Author Jackson Lee, 2014 + +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" + +#include "b3GpuParallelLinearBvh.h" + +b3GpuParallelLinearBvh::b3GpuParallelLinearBvh(cl_context context, cl_device_id device, cl_command_queue queue) : + m_queue(queue), + m_radixSorter(context, device, queue), + + m_rootNodeIndex(context, queue), + m_maxDistanceFromRoot(context, queue), + m_temp(context, queue), + + m_internalNodeAabbs(context, queue), + m_internalNodeLeafIndexRanges(context, queue), + m_internalNodeChildNodes(context, queue), + m_internalNodeParentNodes(context, queue), + + m_commonPrefixes(context, queue), + m_commonPrefixLengths(context, queue), + m_distanceFromRoot(context, queue), + + m_leafNodeParentNodes(context, queue), + m_mortonCodesAndAabbIndicies(context, queue), + m_mergedAabb(context, queue), + m_leafNodeAabbs(context, queue), + + m_largeAabbs(context, queue) +{ + m_rootNodeIndex.resize(1); + m_maxDistanceFromRoot.resize(1); + m_temp.resize(1); + + // + const char CL_PROGRAM_PATH[] = "src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvh.cl"; + + const char* kernelSource = parallelLinearBvhCL; //parallelLinearBvhCL.h + cl_int error; + char* additionalMacros = 0; + m_parallelLinearBvhProgram = b3OpenCLUtils::compileCLProgramFromString(context, device, kernelSource, &error, additionalMacros, CL_PROGRAM_PATH); + b3Assert(m_parallelLinearBvhProgram); + + m_separateAabbsKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "separateAabbs", &error, m_parallelLinearBvhProgram, additionalMacros ); + b3Assert(m_separateAabbsKernel); + m_findAllNodesMergedAabbKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "findAllNodesMergedAabb", &error, m_parallelLinearBvhProgram, additionalMacros ); + b3Assert(m_findAllNodesMergedAabbKernel); + m_assignMortonCodesAndAabbIndiciesKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "assignMortonCodesAndAabbIndicies", &error, m_parallelLinearBvhProgram, additionalMacros ); + b3Assert(m_assignMortonCodesAndAabbIndiciesKernel); + + m_computeAdjacentPairCommonPrefixKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "computeAdjacentPairCommonPrefix", &error, m_parallelLinearBvhProgram, additionalMacros ); + b3Assert(m_computeAdjacentPairCommonPrefixKernel); + m_buildBinaryRadixTreeLeafNodesKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "buildBinaryRadixTreeLeafNodes", &error, m_parallelLinearBvhProgram, additionalMacros ); + b3Assert(m_buildBinaryRadixTreeLeafNodesKernel); + m_buildBinaryRadixTreeInternalNodesKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "buildBinaryRadixTreeInternalNodes", &error, m_parallelLinearBvhProgram, additionalMacros ); + b3Assert(m_buildBinaryRadixTreeInternalNodesKernel); + m_findDistanceFromRootKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "findDistanceFromRoot", &error, m_parallelLinearBvhProgram, additionalMacros ); + b3Assert(m_findDistanceFromRootKernel); + m_buildBinaryRadixTreeAabbsRecursiveKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "buildBinaryRadixTreeAabbsRecursive", &error, m_parallelLinearBvhProgram, additionalMacros ); + b3Assert(m_buildBinaryRadixTreeAabbsRecursiveKernel); + + m_findLeafIndexRangesKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "findLeafIndexRanges", &error, m_parallelLinearBvhProgram, additionalMacros ); + b3Assert(m_findLeafIndexRangesKernel); + + m_plbvhCalculateOverlappingPairsKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "plbvhCalculateOverlappingPairs", &error, m_parallelLinearBvhProgram, additionalMacros ); + b3Assert(m_plbvhCalculateOverlappingPairsKernel); + m_plbvhRayTraverseKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "plbvhRayTraverse", &error, m_parallelLinearBvhProgram, additionalMacros ); + b3Assert(m_plbvhRayTraverseKernel); + m_plbvhLargeAabbAabbTestKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "plbvhLargeAabbAabbTest", &error, m_parallelLinearBvhProgram, additionalMacros ); + b3Assert(m_plbvhLargeAabbAabbTestKernel); + m_plbvhLargeAabbRayTestKernel = b3OpenCLUtils::compileCLKernelFromString( context, device, kernelSource, "plbvhLargeAabbRayTest", &error, m_parallelLinearBvhProgram, additionalMacros ); + b3Assert(m_plbvhLargeAabbRayTestKernel); +} + +b3GpuParallelLinearBvh::~b3GpuParallelLinearBvh() +{ + clReleaseKernel(m_separateAabbsKernel); + clReleaseKernel(m_findAllNodesMergedAabbKernel); + clReleaseKernel(m_assignMortonCodesAndAabbIndiciesKernel); + + clReleaseKernel(m_computeAdjacentPairCommonPrefixKernel); + clReleaseKernel(m_buildBinaryRadixTreeLeafNodesKernel); + clReleaseKernel(m_buildBinaryRadixTreeInternalNodesKernel); + clReleaseKernel(m_findDistanceFromRootKernel); + clReleaseKernel(m_buildBinaryRadixTreeAabbsRecursiveKernel); + + clReleaseKernel(m_findLeafIndexRangesKernel); + + clReleaseKernel(m_plbvhCalculateOverlappingPairsKernel); + clReleaseKernel(m_plbvhRayTraverseKernel); + clReleaseKernel(m_plbvhLargeAabbAabbTestKernel); + clReleaseKernel(m_plbvhLargeAabbRayTestKernel); + + clReleaseProgram(m_parallelLinearBvhProgram); +} + +void b3GpuParallelLinearBvh::build(const b3OpenCLArray& worldSpaceAabbs, const b3OpenCLArray& smallAabbIndices, + const b3OpenCLArray& largeAabbIndices) +{ + B3_PROFILE("b3ParallelLinearBvh::build()"); + + int numLargeAabbs = largeAabbIndices.size(); + int numSmallAabbs = smallAabbIndices.size(); + + //Since all AABBs(both large and small) are input as a contiguous array, + //with 2 additional arrays used to indicate the indices of large and small AABBs, + //it is necessary to separate the AABBs so that the large AABBs will not degrade the quality of the BVH. + { + B3_PROFILE("Separate large and small AABBs"); + + m_largeAabbs.resize(numLargeAabbs); + m_leafNodeAabbs.resize(numSmallAabbs); + + //Write large AABBs into m_largeAabbs + { + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( worldSpaceAabbs.getBufferCL() ), + b3BufferInfoCL( largeAabbIndices.getBufferCL() ), + + b3BufferInfoCL( m_largeAabbs.getBufferCL() ) + }; + + b3LauncherCL launcher(m_queue, m_separateAabbsKernel, "m_separateAabbsKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(numLargeAabbs); + + launcher.launch1D(numLargeAabbs); + } + + //Write small AABBs into m_leafNodeAabbs + { + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( worldSpaceAabbs.getBufferCL() ), + b3BufferInfoCL( smallAabbIndices.getBufferCL() ), + + b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ) + }; + + b3LauncherCL launcher(m_queue, m_separateAabbsKernel, "m_separateAabbsKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(numSmallAabbs); + + launcher.launch1D(numSmallAabbs); + } + + clFinish(m_queue); + } + + // + int numLeaves = numSmallAabbs; //Number of leaves in the BVH == Number of rigid bodies with small AABBs + int numInternalNodes = numLeaves - 1; + + if(numLeaves < 2) + { + //Number of leaf nodes is checked in calculateOverlappingPairs() and testRaysAgainstBvhAabbs(), + //so it does not matter if numLeaves == 0 and rootNodeIndex == -1 + int rootNodeIndex = numLeaves - 1; + m_rootNodeIndex.copyFromHostPointer(&rootNodeIndex, 1); + + //Since the AABBs need to be rearranged(sorted) for the BVH construction algorithm, + //m_mortonCodesAndAabbIndicies.m_value is used to map a sorted AABB index to the unsorted AABB index + //instead of directly moving the AABBs. It needs to be set for the ray cast traversal kernel to work. + //( m_mortonCodesAndAabbIndicies[].m_value == unsorted index == index of m_leafNodeAabbs ) + if(numLeaves == 1) + { + b3SortData leaf; + leaf.m_value = 0; //1 leaf so index is always 0; leaf.m_key does not need to be set + + m_mortonCodesAndAabbIndicies.resize(1); + m_mortonCodesAndAabbIndicies.copyFromHostPointer(&leaf, 1); + } + + return; + } + + // + { + m_internalNodeAabbs.resize(numInternalNodes); + m_internalNodeLeafIndexRanges.resize(numInternalNodes); + m_internalNodeChildNodes.resize(numInternalNodes); + m_internalNodeParentNodes.resize(numInternalNodes); + + m_commonPrefixes.resize(numInternalNodes); + m_commonPrefixLengths.resize(numInternalNodes); + m_distanceFromRoot.resize(numInternalNodes); + + m_leafNodeParentNodes.resize(numLeaves); + m_mortonCodesAndAabbIndicies.resize(numLeaves); + m_mergedAabb.resize(numLeaves); + } + + //Find the merged AABB of all small AABBs; this is used to define the size of + //each cell in the virtual grid for the next kernel(2^10 cells in each dimension). + { + B3_PROFILE("Find AABB of merged nodes"); + + m_mergedAabb.copyFromOpenCLArray(m_leafNodeAabbs); //Need to make a copy since the kernel modifies the array + + for(int numAabbsNeedingMerge = numLeaves; numAabbsNeedingMerge >= 2; + numAabbsNeedingMerge = numAabbsNeedingMerge / 2 + numAabbsNeedingMerge % 2) + { + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_mergedAabb.getBufferCL() ) //Resulting AABB is stored in m_mergedAabb[0] + }; + + b3LauncherCL launcher(m_queue, m_findAllNodesMergedAabbKernel, "m_findAllNodesMergedAabbKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(numAabbsNeedingMerge); + + launcher.launch1D(numAabbsNeedingMerge); + } + + clFinish(m_queue); + } + + //Insert the center of the AABBs into a virtual grid, + //then convert the discrete grid coordinates into a morton code + //For each element in m_mortonCodesAndAabbIndicies, set + // m_key == morton code (value to sort by) + // m_value == small AABB index + { + B3_PROFILE("Assign morton codes"); + + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ), + b3BufferInfoCL( m_mergedAabb.getBufferCL() ), + b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() ) + }; + + b3LauncherCL launcher(m_queue, m_assignMortonCodesAndAabbIndiciesKernel, "m_assignMortonCodesAndAabbIndiciesKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(numLeaves); + + launcher.launch1D(numLeaves); + clFinish(m_queue); + } + + // + { + B3_PROFILE("Sort leaves by morton codes"); + + m_radixSorter.execute(m_mortonCodesAndAabbIndicies); + clFinish(m_queue); + } + + // + constructBinaryRadixTree(); + + + //Since it is a sorted binary radix tree, each internal node contains a contiguous subset of leaf node indices. + //The root node contains leaf node indices in the range [0, numLeafNodes - 1]. + //The child nodes of each node split their parent's index range into 2 contiguous halves. + // + //For example, if the root has indices [0, 31], its children might partition that range into [0, 11] and [12, 31]. + //The next level in the tree could then split those ranges into [0, 2], [3, 11], [12, 22], and [23, 31]. + // + //This property can be used for optimizing calculateOverlappingPairs(), to avoid testing each AABB pair twice + { + B3_PROFILE("m_findLeafIndexRangesKernel"); + + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ), + b3BufferInfoCL( m_internalNodeLeafIndexRanges.getBufferCL() ) + }; + + b3LauncherCL launcher(m_queue, m_findLeafIndexRangesKernel, "m_findLeafIndexRangesKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(numInternalNodes); + + launcher.launch1D(numInternalNodes); + clFinish(m_queue); + } +} + +void b3GpuParallelLinearBvh::calculateOverlappingPairs(b3OpenCLArray& out_overlappingPairs) +{ + int maxPairs = out_overlappingPairs.size(); + b3OpenCLArray& numPairsGpu = m_temp; + + int reset = 0; + numPairsGpu.copyFromHostPointer(&reset, 1); + + // + if( m_leafNodeAabbs.size() > 1 ) + { + B3_PROFILE("PLBVH small-small AABB test"); + + int numQueryAabbs = m_leafNodeAabbs.size(); + + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ), + + b3BufferInfoCL( m_rootNodeIndex.getBufferCL() ), + b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ), + b3BufferInfoCL( m_internalNodeAabbs.getBufferCL() ), + b3BufferInfoCL( m_internalNodeLeafIndexRanges.getBufferCL() ), + b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() ), + + b3BufferInfoCL( numPairsGpu.getBufferCL() ), + b3BufferInfoCL( out_overlappingPairs.getBufferCL() ) + }; + + b3LauncherCL launcher(m_queue, m_plbvhCalculateOverlappingPairsKernel, "m_plbvhCalculateOverlappingPairsKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(maxPairs); + launcher.setConst(numQueryAabbs); + + launcher.launch1D(numQueryAabbs); + clFinish(m_queue); + } + + int numLargeAabbRigids = m_largeAabbs.size(); + if( numLargeAabbRigids > 0 && m_leafNodeAabbs.size() > 0 ) + { + B3_PROFILE("PLBVH large-small AABB test"); + + int numQueryAabbs = m_leafNodeAabbs.size(); + + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ), + b3BufferInfoCL( m_largeAabbs.getBufferCL() ), + + b3BufferInfoCL( numPairsGpu.getBufferCL() ), + b3BufferInfoCL( out_overlappingPairs.getBufferCL() ) + }; + + b3LauncherCL launcher(m_queue, m_plbvhLargeAabbAabbTestKernel, "m_plbvhLargeAabbAabbTestKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(maxPairs); + launcher.setConst(numLargeAabbRigids); + launcher.setConst(numQueryAabbs); + + launcher.launch1D(numQueryAabbs); + clFinish(m_queue); + } + + + // + int numPairs = -1; + numPairsGpu.copyToHostPointer(&numPairs, 1); + if(numPairs > maxPairs) + { + b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs); + numPairs = maxPairs; + numPairsGpu.copyFromHostPointer(&maxPairs, 1); + } + + out_overlappingPairs.resize(numPairs); +} + + +void b3GpuParallelLinearBvh::testRaysAgainstBvhAabbs(const b3OpenCLArray& rays, + b3OpenCLArray& out_numRayRigidPairs, b3OpenCLArray& out_rayRigidPairs) +{ + B3_PROFILE("PLBVH testRaysAgainstBvhAabbs()"); + + int numRays = rays.size(); + int maxRayRigidPairs = out_rayRigidPairs.size(); + + int reset = 0; + out_numRayRigidPairs.copyFromHostPointer(&reset, 1); + + // + if( m_leafNodeAabbs.size() > 0 ) + { + B3_PROFILE("PLBVH ray test small AABB"); + + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ), + + b3BufferInfoCL( m_rootNodeIndex.getBufferCL() ), + b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ), + b3BufferInfoCL( m_internalNodeAabbs.getBufferCL() ), + b3BufferInfoCL( m_internalNodeLeafIndexRanges.getBufferCL() ), + b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() ), + + b3BufferInfoCL( rays.getBufferCL() ), + + b3BufferInfoCL( out_numRayRigidPairs.getBufferCL() ), + b3BufferInfoCL( out_rayRigidPairs.getBufferCL() ) + }; + + b3LauncherCL launcher(m_queue, m_plbvhRayTraverseKernel, "m_plbvhRayTraverseKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(maxRayRigidPairs); + launcher.setConst(numRays); + + launcher.launch1D(numRays); + clFinish(m_queue); + } + + int numLargeAabbRigids = m_largeAabbs.size(); + if(numLargeAabbRigids > 0) + { + B3_PROFILE("PLBVH ray test large AABB"); + + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_largeAabbs.getBufferCL() ), + b3BufferInfoCL( rays.getBufferCL() ), + + b3BufferInfoCL( out_numRayRigidPairs.getBufferCL() ), + b3BufferInfoCL( out_rayRigidPairs.getBufferCL() ) + }; + + b3LauncherCL launcher(m_queue, m_plbvhLargeAabbRayTestKernel, "m_plbvhLargeAabbRayTestKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(numLargeAabbRigids); + launcher.setConst(maxRayRigidPairs); + launcher.setConst(numRays); + + launcher.launch1D(numRays); + clFinish(m_queue); + } + + // + int numRayRigidPairs = -1; + out_numRayRigidPairs.copyToHostPointer(&numRayRigidPairs, 1); + + if(numRayRigidPairs > maxRayRigidPairs) + b3Error("Error running out of rayRigid pairs: numRayRigidPairs = %d, maxRayRigidPairs = %d.\n", numRayRigidPairs, maxRayRigidPairs); + +} + +void b3GpuParallelLinearBvh::constructBinaryRadixTree() +{ + B3_PROFILE("b3GpuParallelLinearBvh::constructBinaryRadixTree()"); + + int numLeaves = m_leafNodeAabbs.size(); + int numInternalNodes = numLeaves - 1; + + //Each internal node is placed in between 2 leaf nodes. + //By using this arrangement and computing the common prefix between + //these 2 adjacent leaf nodes, it is possible to quickly construct a binary radix tree. + { + B3_PROFILE("m_computeAdjacentPairCommonPrefixKernel"); + + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() ), + b3BufferInfoCL( m_commonPrefixes.getBufferCL() ), + b3BufferInfoCL( m_commonPrefixLengths.getBufferCL() ) + }; + + b3LauncherCL launcher(m_queue, m_computeAdjacentPairCommonPrefixKernel, "m_computeAdjacentPairCommonPrefixKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(numInternalNodes); + + launcher.launch1D(numInternalNodes); + clFinish(m_queue); + } + + //For each leaf node, select its parent node by + //comparing the 2 nearest internal nodes and assign child node indices + { + B3_PROFILE("m_buildBinaryRadixTreeLeafNodesKernel"); + + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_commonPrefixLengths.getBufferCL() ), + b3BufferInfoCL( m_leafNodeParentNodes.getBufferCL() ), + b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ) + }; + + b3LauncherCL launcher(m_queue, m_buildBinaryRadixTreeLeafNodesKernel, "m_buildBinaryRadixTreeLeafNodesKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(numLeaves); + + launcher.launch1D(numLeaves); + clFinish(m_queue); + } + + //For each internal node, perform 2 binary searches among the other internal nodes + //to its left and right to find its potential parent nodes and assign child node indices + { + B3_PROFILE("m_buildBinaryRadixTreeInternalNodesKernel"); + + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_commonPrefixes.getBufferCL() ), + b3BufferInfoCL( m_commonPrefixLengths.getBufferCL() ), + b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ), + b3BufferInfoCL( m_internalNodeParentNodes.getBufferCL() ), + b3BufferInfoCL( m_rootNodeIndex.getBufferCL() ) + }; + + b3LauncherCL launcher(m_queue, m_buildBinaryRadixTreeInternalNodesKernel, "m_buildBinaryRadixTreeInternalNodesKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(numInternalNodes); + + launcher.launch1D(numInternalNodes); + clFinish(m_queue); + } + + //Find the number of nodes seperating each internal node and the root node + //so that the AABBs can be set using the next kernel. + //Also determine the maximum number of nodes separating an internal node and the root node. + { + B3_PROFILE("m_findDistanceFromRootKernel"); + + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_rootNodeIndex.getBufferCL() ), + b3BufferInfoCL( m_internalNodeParentNodes.getBufferCL() ), + b3BufferInfoCL( m_maxDistanceFromRoot.getBufferCL() ), + b3BufferInfoCL( m_distanceFromRoot.getBufferCL() ) + }; + + b3LauncherCL launcher(m_queue, m_findDistanceFromRootKernel, "m_findDistanceFromRootKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(numInternalNodes); + + launcher.launch1D(numInternalNodes); + clFinish(m_queue); + } + + //Starting from the internal nodes nearest to the leaf nodes, recursively move up + //the tree towards the root to set the AABBs of each internal node; each internal node + //checks its children and merges their AABBs + { + B3_PROFILE("m_buildBinaryRadixTreeAabbsRecursiveKernel"); + + int maxDistanceFromRoot = -1; + { + B3_PROFILE("copy maxDistanceFromRoot to CPU"); + m_maxDistanceFromRoot.copyToHostPointer(&maxDistanceFromRoot, 1); + clFinish(m_queue); + } + + for(int distanceFromRoot = maxDistanceFromRoot; distanceFromRoot >= 0; --distanceFromRoot) + { + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_distanceFromRoot.getBufferCL() ), + b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() ), + b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() ), + b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() ), + b3BufferInfoCL( m_internalNodeAabbs.getBufferCL() ) + }; + + b3LauncherCL launcher(m_queue, m_buildBinaryRadixTreeAabbsRecursiveKernel, "m_buildBinaryRadixTreeAabbsRecursiveKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(maxDistanceFromRoot); + launcher.setConst(distanceFromRoot); + launcher.setConst(numInternalNodes); + + //It may seem inefficent to launch a thread for each internal node when a + //much smaller number of nodes is actually processed, but this is actually + //faster than determining the exact nodes that are ready to merge their child AABBs. + launcher.launch1D(numInternalNodes); + } + + clFinish(m_queue); + } +} + + \ No newline at end of file diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.h b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.h new file mode 100644 index 000000000000..effe617b7b8c --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.h @@ -0,0 +1,125 @@ +/* +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. +*/ +//Initial Author Jackson Lee, 2014 + +#ifndef B3_GPU_PARALLEL_LINEAR_BVH_H +#define B3_GPU_PARALLEL_LINEAR_BVH_H + +//#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h" +#include "Bullet3Common/shared/b3Int2.h" +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h" + +#include "Bullet3OpenCL/ParallelPrimitives/b3FillCL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h" + +#include "Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvhKernels.h" + +#define b3Int64 cl_long + +///@brief GPU Parallel Linearized Bounding Volume Heirarchy(LBVH) that is reconstructed every frame +///@remarks +///See presentation in docs/b3GpuParallelLinearBvh.pdf for algorithm details. +///@par +///Related papers: \n +///"Fast BVH Construction on GPUs" [Lauterbach et al. 2009] \n +///"Maximizing Parallelism in the Construction of BVHs, Octrees, and k-d trees" [Karras 2012] \n +///@par +///The basic algorithm for building the BVH as presented in [Lauterbach et al. 2009] consists of 4 stages: +/// - [fully parallel] Assign morton codes for each AABB using its center (after quantizing the AABB centers into a virtual grid) +/// - [fully parallel] Sort morton codes +/// - [somewhat parallel] Build binary radix tree (assign parent/child pointers for internal nodes of the BVH) +/// - [somewhat parallel] Set internal node AABBs +///@par +///[Karras 2012] improves on the algorithm by introducing fully parallel methods for the last 2 stages. +///The BVH implementation here shares many concepts with [Karras 2012], but a different method is used for constructing the tree. +///Instead of searching for the child nodes of each internal node, we search for the parent node of each node. +///Additionally, a non-atomic traversal that starts from the leaf nodes and moves towards the root node is used to set the AABBs. +class b3GpuParallelLinearBvh +{ + cl_command_queue m_queue; + + cl_program m_parallelLinearBvhProgram; + + cl_kernel m_separateAabbsKernel; + cl_kernel m_findAllNodesMergedAabbKernel; + cl_kernel m_assignMortonCodesAndAabbIndiciesKernel; + + //Binary radix tree construction kernels + cl_kernel m_computeAdjacentPairCommonPrefixKernel; + cl_kernel m_buildBinaryRadixTreeLeafNodesKernel; + cl_kernel m_buildBinaryRadixTreeInternalNodesKernel; + cl_kernel m_findDistanceFromRootKernel; + cl_kernel m_buildBinaryRadixTreeAabbsRecursiveKernel; + + cl_kernel m_findLeafIndexRangesKernel; + + //Traversal kernels + cl_kernel m_plbvhCalculateOverlappingPairsKernel; + cl_kernel m_plbvhRayTraverseKernel; + cl_kernel m_plbvhLargeAabbAabbTestKernel; + cl_kernel m_plbvhLargeAabbRayTestKernel; + + b3RadixSort32CL m_radixSorter; + + //1 element + b3OpenCLArray m_rootNodeIndex; //Most significant bit(0x80000000) is set to indicate internal node + b3OpenCLArray m_maxDistanceFromRoot; //Max number of internal nodes between an internal node and the root node + b3OpenCLArray m_temp; //Used to hold the number of pairs in calculateOverlappingPairs() + + //1 element per internal node (number_of_internal_nodes == number_of_leaves - 1) + b3OpenCLArray m_internalNodeAabbs; + b3OpenCLArray m_internalNodeLeafIndexRanges; //x == min leaf index, y == max leaf index + b3OpenCLArray m_internalNodeChildNodes; //x == left child, y == right child; msb(0x80000000) is set to indicate internal node + b3OpenCLArray m_internalNodeParentNodes; //For parent node index, msb(0x80000000) is not set since it is always internal + + //1 element per internal node; for binary radix tree construction + b3OpenCLArray m_commonPrefixes; + b3OpenCLArray m_commonPrefixLengths; + b3OpenCLArray m_distanceFromRoot; //Number of internal nodes between this node and the root + + //1 element per leaf node (leaf nodes only include small AABBs) + b3OpenCLArray m_leafNodeParentNodes; //For parent node index, msb(0x80000000) is not set since it is always internal + b3OpenCLArray m_mortonCodesAndAabbIndicies; //m_key == morton code, m_value == aabb index in m_leafNodeAabbs + b3OpenCLArray m_mergedAabb; //m_mergedAabb[0] contains the merged AABB of all leaf nodes + b3OpenCLArray m_leafNodeAabbs; //Contains only small AABBs + + //1 element per large AABB, which is not stored in the BVH + b3OpenCLArray m_largeAabbs; + +public: + b3GpuParallelLinearBvh(cl_context context, cl_device_id device, cl_command_queue queue); + virtual ~b3GpuParallelLinearBvh(); + + ///Must be called before any other function + void build(const b3OpenCLArray& worldSpaceAabbs, const b3OpenCLArray& smallAabbIndices, + const b3OpenCLArray& largeAabbIndices); + + ///calculateOverlappingPairs() uses the worldSpaceAabbs parameter of b3GpuParallelLinearBvh::build() as the query AABBs. + ///@param out_overlappingPairs The size() of this array is used to determine the max number of pairs. + ///If the number of overlapping pairs is < out_overlappingPairs.size(), out_overlappingPairs is resized. + void calculateOverlappingPairs(b3OpenCLArray& out_overlappingPairs); + + ///@param out_numRigidRayPairs Array of length 1; contains the number of detected ray-rigid AABB intersections; + ///this value may be greater than out_rayRigidPairs.size() if out_rayRigidPairs is not large enough. + ///@param out_rayRigidPairs Contains an array of rays intersecting rigid AABBs; x == ray index, y == rigid body index. + ///If the size of this array is insufficient to hold all ray-rigid AABB intersections, additional intersections are discarded. + void testRaysAgainstBvhAabbs(const b3OpenCLArray& rays, + b3OpenCLArray& out_numRayRigidPairs, b3OpenCLArray& out_rayRigidPairs); + +private: + void constructBinaryRadixTree(); +}; + +#endif diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.cpp b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.cpp new file mode 100644 index 000000000000..d2618024ac4b --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.cpp @@ -0,0 +1,80 @@ +/* +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. +*/ +//Initial Author Jackson Lee, 2014 + +#include "b3GpuParallelLinearBvhBroadphase.h" + +b3GpuParallelLinearBvhBroadphase::b3GpuParallelLinearBvhBroadphase(cl_context context, cl_device_id device, cl_command_queue queue) : + m_plbvh(context, device, queue), + + m_overlappingPairsGpu(context, queue), + + m_aabbsGpu(context, queue), + m_smallAabbsMappingGpu(context, queue), + m_largeAabbsMappingGpu(context, queue) +{ +} + +void b3GpuParallelLinearBvhBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask) +{ + int newAabbIndex = m_aabbsCpu.size(); + + b3SapAabb aabb; + aabb.m_minVec = aabbMin; + aabb.m_maxVec = aabbMax; + + aabb.m_minIndices[3] = userPtr; + aabb.m_signedMaxIndices[3] = newAabbIndex; + + m_smallAabbsMappingCpu.push_back(newAabbIndex); + + m_aabbsCpu.push_back(aabb); +} +void b3GpuParallelLinearBvhBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask) +{ + int newAabbIndex = m_aabbsCpu.size(); + + b3SapAabb aabb; + aabb.m_minVec = aabbMin; + aabb.m_maxVec = aabbMax; + + aabb.m_minIndices[3] = userPtr; + aabb.m_signedMaxIndices[3] = newAabbIndex; + + m_largeAabbsMappingCpu.push_back(newAabbIndex); + + m_aabbsCpu.push_back(aabb); +} + +void b3GpuParallelLinearBvhBroadphase::calculateOverlappingPairs(int maxPairs) +{ + //Reconstruct BVH + m_plbvh.build(m_aabbsGpu, m_smallAabbsMappingGpu, m_largeAabbsMappingGpu); + + // + m_overlappingPairsGpu.resize(maxPairs); + m_plbvh.calculateOverlappingPairs(m_overlappingPairsGpu); +} +void b3GpuParallelLinearBvhBroadphase::calculateOverlappingPairsHost(int maxPairs) +{ + b3Assert(0); //CPU version not implemented +} + +void b3GpuParallelLinearBvhBroadphase::writeAabbsToGpu() +{ + m_aabbsGpu.copyFromHost(m_aabbsCpu); + m_smallAabbsMappingGpu.copyFromHost(m_smallAabbsMappingCpu); + m_largeAabbsMappingGpu.copyFromHost(m_largeAabbsMappingCpu); +} + + + diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.h b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.h new file mode 100644 index 000000000000..e5185006371d --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.h @@ -0,0 +1,66 @@ +/* +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. +*/ +//Initial Author Jackson Lee, 2014 + +#ifndef B3_GPU_PARALLEL_LINEAR_BVH_BROADPHASE_H +#define B3_GPU_PARALLEL_LINEAR_BVH_BROADPHASE_H + +#include "Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h" + +#include "b3GpuParallelLinearBvh.h" + +class b3GpuParallelLinearBvhBroadphase : public b3GpuBroadphaseInterface +{ + b3GpuParallelLinearBvh m_plbvh; + + b3OpenCLArray m_overlappingPairsGpu; + + b3OpenCLArray m_aabbsGpu; + b3OpenCLArray m_smallAabbsMappingGpu; + b3OpenCLArray m_largeAabbsMappingGpu; + + b3AlignedObjectArray m_aabbsCpu; + b3AlignedObjectArray m_smallAabbsMappingCpu; + b3AlignedObjectArray m_largeAabbsMappingCpu; + +public: + b3GpuParallelLinearBvhBroadphase(cl_context context, cl_device_id device, cl_command_queue queue); + virtual ~b3GpuParallelLinearBvhBroadphase() {} + + virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask); + virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask); + + virtual void calculateOverlappingPairs(int maxPairs); + virtual void calculateOverlappingPairsHost(int maxPairs); + + //call writeAabbsToGpu after done making all changes (createProxy etc) + virtual void writeAabbsToGpu(); + + virtual int getNumOverlap() { return m_overlappingPairsGpu.size(); } + virtual cl_mem getOverlappingPairBuffer() { return m_overlappingPairsGpu.getBufferCL(); } + + virtual cl_mem getAabbBufferWS() { return m_aabbsGpu.getBufferCL(); } + virtual b3OpenCLArray& getAllAabbsGPU() { return m_aabbsGpu; } + + virtual b3OpenCLArray& getOverlappingPairsGPU() { return m_overlappingPairsGpu; } + virtual b3OpenCLArray& getSmallAabbIndicesGPU() { return m_smallAabbsMappingGpu; } + virtual b3OpenCLArray& getLargeAabbIndicesGPU() { return m_largeAabbsMappingGpu; } + + virtual b3AlignedObjectArray& getAllAabbsCPU() { return m_aabbsCpu; } + + static b3GpuBroadphaseInterface* CreateFunc(cl_context context, cl_device_id device, cl_command_queue queue) + { + return new b3GpuParallelLinearBvhBroadphase(context, device, queue); + } +}; + +#endif diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp new file mode 100644 index 000000000000..c45fbbdcaa30 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp @@ -0,0 +1,1366 @@ + +bool searchIncremental3dSapOnGpu = true; +#include +#include "b3GpuSapBroadphase.h" +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanFloat4CL.h" + + +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "kernels/sapKernels.h" + +#include "Bullet3Common/b3MinMax.h" + +#define B3_BROADPHASE_SAP_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl" + +/* + + + + + + + b3OpenCLArray m_pairCount; + + + b3OpenCLArray m_allAabbsGPU; + b3AlignedObjectArray m_allAabbsCPU; + + virtual b3OpenCLArray& getAllAabbsGPU() + { + return m_allAabbsGPU; + } + virtual b3AlignedObjectArray& getAllAabbsCPU() + { + return m_allAabbsCPU; + } + + b3OpenCLArray m_sum; + b3OpenCLArray m_sum2; + b3OpenCLArray m_dst; + + b3OpenCLArray m_smallAabbsMappingGPU; + b3AlignedObjectArray m_smallAabbsMappingCPU; + + b3OpenCLArray m_largeAabbsMappingGPU; + b3AlignedObjectArray m_largeAabbsMappingCPU; + + + b3OpenCLArray m_overlappingPairs; + + //temporary gpu work memory + b3OpenCLArray m_gpuSmallSortData; + b3OpenCLArray m_gpuSmallSortedAabbs; + + class b3PrefixScanFloat4CL* m_prefixScanFloat4; + */ + +b3GpuSapBroadphase::b3GpuSapBroadphase(cl_context ctx,cl_device_id device, cl_command_queue q , b3GpuSapKernelType kernelType) +:m_context(ctx), +m_device(device), +m_queue(q), + +m_objectMinMaxIndexGPUaxis0(ctx,q), +m_objectMinMaxIndexGPUaxis1(ctx,q), +m_objectMinMaxIndexGPUaxis2(ctx,q), +m_objectMinMaxIndexGPUaxis0prev(ctx,q), +m_objectMinMaxIndexGPUaxis1prev(ctx,q), +m_objectMinMaxIndexGPUaxis2prev(ctx,q), +m_sortedAxisGPU0(ctx,q), +m_sortedAxisGPU1(ctx,q), +m_sortedAxisGPU2(ctx,q), +m_sortedAxisGPU0prev(ctx,q), +m_sortedAxisGPU1prev(ctx,q), +m_sortedAxisGPU2prev(ctx,q), +m_addedHostPairsGPU(ctx,q), +m_removedHostPairsGPU(ctx,q), +m_addedCountGPU(ctx,q), +m_removedCountGPU(ctx,q), +m_currentBuffer(-1), +m_pairCount(ctx,q), +m_allAabbsGPU(ctx,q), +m_sum(ctx,q), +m_sum2(ctx,q), +m_dst(ctx,q), +m_smallAabbsMappingGPU(ctx,q), +m_largeAabbsMappingGPU(ctx,q), +m_overlappingPairs(ctx,q), +m_gpuSmallSortData(ctx,q), +m_gpuSmallSortedAabbs(ctx,q) +{ + const char* sapSrc = sapCL; + + + cl_int errNum=0; + + b3Assert(m_context); + b3Assert(m_device); + cl_program sapProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,sapSrc,&errNum,"",B3_BROADPHASE_SAP_PATH); + b3Assert(errNum==CL_SUCCESS); + + + b3Assert(errNum==CL_SUCCESS); +#ifndef __APPLE__ + m_prefixScanFloat4 = new b3PrefixScanFloat4CL(m_context,m_device,m_queue); +#else + m_prefixScanFloat4 = 0; +#endif + m_sapKernel = 0; + + switch (kernelType) + { + case B3_GPU_SAP_KERNEL_BRUTE_FORCE_CPU: + { + m_sapKernel=0; + break; + } + case B3_GPU_SAP_KERNEL_BRUTE_FORCE_GPU: + { + m_sapKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "computePairsKernelBruteForce",&errNum,sapProg ); + break; + } + + case B3_GPU_SAP_KERNEL_ORIGINAL: + { + m_sapKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "computePairsKernelOriginal",&errNum,sapProg ); + break; + } + case B3_GPU_SAP_KERNEL_BARRIER: + { + m_sapKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "computePairsKernelBarrier",&errNum,sapProg ); + break; + } + case B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY: + { + m_sapKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "computePairsKernelLocalSharedMemory",&errNum,sapProg ); + break; + } + + default: + { + m_sapKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "computePairsKernelLocalSharedMemory",&errNum,sapProg ); + b3Error("Unknown 3D GPU SAP provided, fallback to computePairsKernelLocalSharedMemory"); + } + }; + + + + m_sap2Kernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "computePairsKernelTwoArrays",&errNum,sapProg ); + b3Assert(errNum==CL_SUCCESS); + + m_prepareSumVarianceKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "prepareSumVarianceKernel",&errNum,sapProg ); + b3Assert(errNum==CL_SUCCESS); + + + m_flipFloatKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "flipFloatKernel",&errNum,sapProg ); + + m_copyAabbsKernel= b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "copyAabbsKernel",&errNum,sapProg ); + + m_scatterKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,sapSrc, "scatterKernel",&errNum,sapProg ); + + m_sorter = new b3RadixSort32CL(m_context,m_device,m_queue); +} + +b3GpuSapBroadphase::~b3GpuSapBroadphase() +{ + delete m_sorter; + delete m_prefixScanFloat4; + + clReleaseKernel(m_scatterKernel); + clReleaseKernel(m_flipFloatKernel); + clReleaseKernel(m_copyAabbsKernel); + clReleaseKernel(m_sapKernel); + clReleaseKernel(m_sap2Kernel); + clReleaseKernel(m_prepareSumVarianceKernel); + + +} + +/// conservative test for overlap between two aabbs +static bool TestAabbAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aabbMax1, + const b3Vector3 &aabbMin2, const b3Vector3 &aabbMax2) +{ + bool overlap = true; + overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap; + overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap; + overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap; + return overlap; +} + + + +//http://stereopsis.com/radix.html +static unsigned int FloatFlip(float fl) +{ + unsigned int f = *(unsigned int*)&fl; + unsigned int mask = -(int)(f >> 31) | 0x80000000; + return f ^ mask; +}; + +void b3GpuSapBroadphase::init3dSap() +{ + if (m_currentBuffer<0) + { + m_allAabbsGPU.copyToHost(m_allAabbsCPU); + + m_currentBuffer = 0; + for (int axis=0;axis<3;axis++) + { + for (int buf=0;buf<2;buf++) + { + int totalNumAabbs = m_allAabbsCPU.size(); + int numEndPoints = 2*totalNumAabbs; + m_sortedAxisCPU[axis][buf].resize(numEndPoints); + + if (buf==m_currentBuffer) + { + for (int i=0;iexecuteHost(m_sortedAxisCPU[axis][m_currentBuffer]); + } + + for (int axis=0;axis<3;axis++) + { + //int totalNumAabbs = m_allAabbsCPU.size(); + int numEndPoints = m_sortedAxisCPU[axis][m_currentBuffer].size(); + m_objectMinMaxIndexCPU[axis][m_currentBuffer].resize(numEndPoints); + for (int i=0;i(const b3Int4& a,const b3Int4& b) +{ + return a.x > b.x || (a.x == b.x && a.y > b.y); +}; + +b3AlignedObjectArray addedHostPairs; +b3AlignedObjectArray removedHostPairs; + +b3AlignedObjectArray preAabbs; + +void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap() +{ + //static int framepje = 0; + //printf("framepje=%d\n",framepje++); + + + B3_PROFILE("calculateOverlappingPairsHostIncremental3Sap"); + + addedHostPairs.resize(0); + removedHostPairs.resize(0); + + b3Assert(m_currentBuffer>=0); + + { + preAabbs.resize(m_allAabbsCPU.size()); + for (int i=0;i allPairs; + { + B3_PROFILE("m_overlappingPairs.copyToHost"); + m_overlappingPairs.copyToHost(allPairs); + } + if (0) + { + { + printf("ab[40].min=%f,%f,%f,ab[40].max=%f,%f,%f\n", + m_allAabbsCPU[40].m_min[0], m_allAabbsCPU[40].m_min[1],m_allAabbsCPU[40].m_min[2], + m_allAabbsCPU[40].m_max[0], m_allAabbsCPU[40].m_max[1],m_allAabbsCPU[40].m_max[2]); + } + + { + printf("ab[53].min=%f,%f,%f,ab[53].max=%f,%f,%f\n", + m_allAabbsCPU[53].m_min[0], m_allAabbsCPU[53].m_min[1],m_allAabbsCPU[53].m_min[2], + m_allAabbsCPU[53].m_max[0], m_allAabbsCPU[53].m_max[1],m_allAabbsCPU[53].m_max[2]); + } + + + { + b3Int4 newPair; + newPair.x = 40; + newPair.y = 53; + int index = allPairs.findBinarySearch(newPair); + printf("hasPair(40,53)=%d out of %d\n",index, allPairs.size()); + + { + int overlap = TestAabbAgainstAabb2((const b3Vector3&)m_allAabbsCPU[40].m_min, (const b3Vector3&)m_allAabbsCPU[40].m_max,(const b3Vector3&)m_allAabbsCPU[53].m_min,(const b3Vector3&)m_allAabbsCPU[53].m_max); + printf("overlap=%d\n",overlap); + } + + if (preAabbs.size()) + { + int prevOverlap = TestAabbAgainstAabb2((const b3Vector3&)preAabbs[40].m_min, (const b3Vector3&)preAabbs[40].m_max,(const b3Vector3&)preAabbs[53].m_min,(const b3Vector3&)preAabbs[53].m_max); + printf("prevoverlap=%d\n",prevOverlap); + } else + { + printf("unknown prevoverlap\n"); + } + + } + } + + + if (0) + { + for (int i=0;iexecuteHost(m_sortedAxisCPU[axis][m_currentBuffer]); + } + +#if 0 + if (0) + { + for (int axis=0;axis<3;axis++) + { + //printf("axis %d\n",axis); + for (int i=0;i m_objectMinMaxIndexCPU[ax][m_currentBuffer][otherIndex].y) || + (m_objectMinMaxIndexCPU[ax][m_currentBuffer][i].y < m_objectMinMaxIndexCPU[ax][m_currentBuffer][otherIndex].x)) + overlap=false; + } + + // b3Assert(overlap2==overlap); + + bool prevOverlap = true; + + for (int ax=0;ax<3;ax++) + { + if ((m_objectMinMaxIndexCPU[ax][1-m_currentBuffer][i].x > m_objectMinMaxIndexCPU[ax][1-m_currentBuffer][otherIndex].y) || + (m_objectMinMaxIndexCPU[ax][1-m_currentBuffer][i].y < m_objectMinMaxIndexCPU[ax][1-m_currentBuffer][otherIndex].x)) + prevOverlap=false; + } + + + //b3Assert(overlap==overlap2); + + + + if (dmin<0) + { + if (overlap && !prevOverlap) + { + //add a pair + b3Int4 newPair; + if (i<=otherIndex) + { + newPair.x = i; + newPair.y = otherIndex; + } else + { + newPair.x = otherIndex; + newPair.y = i; + } + addedHostPairs.push_back(newPair); + } + } + else + { + if (!overlap && prevOverlap) + { + + //remove a pair + b3Int4 removedPair; + if (i<=otherIndex) + { + removedPair.x = i; + removedPair.y = otherIndex; + } else + { + removedPair.x = otherIndex; + removedPair.y = i; + } + removedHostPairs.push_back(removedPair); + } + }//otherisMax + }//if (dmin<0) + }//if (otherIndex!=i) + }//for (int j= + } + + if (dmax!=0) + { + int stepMax = dmax<0 ? -1 : 1; + for (int j=prevMaxIndex;j!=curMaxIndex;j+=stepMax) + { + int otherIndex2 = m_sortedAxisCPU[axis][otherbuffer][j].y; + int otherIndex = otherIndex2/2; + if (otherIndex!=i) + { + //bool otherIsMin = ((otherIndex2&1)==0); + //if (otherIsMin) + { + //bool overlap = TestAabbAgainstAabb2((const b3Vector3&)m_allAabbsCPU[i].m_min, (const b3Vector3&)m_allAabbsCPU[i].m_max,(const b3Vector3&)m_allAabbsCPU[otherIndex].m_min,(const b3Vector3&)m_allAabbsCPU[otherIndex].m_max); + //bool prevOverlap = TestAabbAgainstAabb2((const b3Vector3&)preAabbs[i].m_min, (const b3Vector3&)preAabbs[i].m_max,(const b3Vector3&)preAabbs[otherIndex].m_min,(const b3Vector3&)preAabbs[otherIndex].m_max); + + bool overlap = true; + + for (int ax=0;ax<3;ax++) + { + if ((m_objectMinMaxIndexCPU[ax][m_currentBuffer][i].x > m_objectMinMaxIndexCPU[ax][m_currentBuffer][otherIndex].y) || + (m_objectMinMaxIndexCPU[ax][m_currentBuffer][i].y < m_objectMinMaxIndexCPU[ax][m_currentBuffer][otherIndex].x)) + overlap=false; + } + //b3Assert(overlap2==overlap); + + bool prevOverlap = true; + + for (int ax=0;ax<3;ax++) + { + if ((m_objectMinMaxIndexCPU[ax][1-m_currentBuffer][i].x > m_objectMinMaxIndexCPU[ax][1-m_currentBuffer][otherIndex].y) || + (m_objectMinMaxIndexCPU[ax][1-m_currentBuffer][i].y < m_objectMinMaxIndexCPU[ax][1-m_currentBuffer][otherIndex].x)) + prevOverlap=false; + } + + + if (dmax>0) + { + if (overlap && !prevOverlap) + { + //add a pair + b3Int4 newPair; + if (i<=otherIndex) + { + newPair.x = i; + newPair.y = otherIndex; + } else + { + newPair.x = otherIndex; + newPair.y = i; + } + addedHostPairs.push_back(newPair); + + } + } + else + { + if (!overlap && prevOverlap) + { + //if (otherIndex2&1==0) -> min? + //remove a pair + b3Int4 removedPair; + if (i<=otherIndex) + { + removedPair.x = i; + removedPair.y = otherIndex; + } else + { + removedPair.x = otherIndex; + removedPair.y = i; + } + removedHostPairs.push_back(removedPair); + + } + } + + }//if (dmin<0) + }//if (otherIndex!=i) + }//for (int j= + } + }//for (int otherbuffer + }//for (int axis=0; + }//for (int i=0;i removedPositions; + + { + B3_PROFILE("actual removing"); + for (int i=0;i actualAddedPairs; + + { + B3_PROFILE("actual adding"); + for (int i=0;i=0) + // return calculateOverlappingPairsHostIncremental3Sap(); + + b3Assert(m_allAabbsCPU.size() == m_allAabbsGPU.size()); + m_allAabbsGPU.copyToHost(m_allAabbsCPU); + + + + int axis=0; + { + B3_PROFILE("CPU compute best variance axis"); + b3Vector3 s=b3MakeVector3(0,0,0),s2=b3MakeVector3(0,0,0); + int numRigidBodies = m_smallAabbsMappingCPU.size(); + + for(int i=0;im_allAabbsCPU[m_smallAabbsMappingCPU[i]]; + + b3Vector3 maxAabb=b3MakeVector3(aabb.m_max[0],aabb.m_max[1],aabb.m_max[2]); + b3Vector3 minAabb=b3MakeVector3(aabb.m_min[0],aabb.m_min[1],aabb.m_min[2]); + b3Vector3 centerAabb=(maxAabb+minAabb)*0.5f; + + s += centerAabb; + s2 += centerAabb*centerAabb; + } + b3Vector3 v = s2 - (s*s) / (float)numRigidBodies; + + if(v[1] > v[0]) + axis = 1; + if(v[2] > v[axis]) + axis = 2; + } + + + + + b3AlignedObjectArray hostPairs; + + { + int numSmallAabbs = m_smallAabbsMappingCPU.size(); + for (int i=0;i maxPairs) + { + hostPairs.resize(maxPairs); + } + + if (hostPairs.size()) + { + m_overlappingPairs.copyFromHost(hostPairs); + } else + { + m_overlappingPairs.resize(0); + } + + //init3dSap(); + +} + +void b3GpuSapBroadphase::reset() +{ + m_allAabbsGPU.resize(0); + m_allAabbsCPU.resize(0); + + + m_smallAabbsMappingGPU.resize(0); + m_smallAabbsMappingCPU.resize(0); + + m_pairCount.resize(0); + + m_largeAabbsMappingGPU.resize(0); + m_largeAabbsMappingCPU.resize(0); + +} + + +void b3GpuSapBroadphase::calculateOverlappingPairs(int maxPairs) +{ + if (m_sapKernel==0) + { + calculateOverlappingPairsHost(maxPairs); + return; + } + + //if (m_currentBuffer>=0) + // return calculateOverlappingPairsHostIncremental3Sap(); + + //calculateOverlappingPairsHost(maxPairs); + + B3_PROFILE("GPU 1-axis SAP calculateOverlappingPairs"); + + int axis = 0; + + { + + //bool syncOnHost = false; + + int numSmallAabbs = m_smallAabbsMappingCPU.size(); + if (m_prefixScanFloat4 && numSmallAabbs) + { + B3_PROFILE("GPU compute best variance axis"); + + if (m_dst.size()!=(numSmallAabbs+1)) + { + m_dst.resize(numSmallAabbs+128); + m_sum.resize(numSmallAabbs+128); + m_sum2.resize(numSmallAabbs+128); + m_sum.at(numSmallAabbs)=b3MakeVector3(0,0,0); //slow? + m_sum2.at(numSmallAabbs)=b3MakeVector3(0,0,0); //slow? + } + + b3LauncherCL launcher(m_queue, m_prepareSumVarianceKernel ,"m_prepareSumVarianceKernel"); + launcher.setBuffer(m_allAabbsGPU.getBufferCL()); + + launcher.setBuffer(m_smallAabbsMappingGPU.getBufferCL()); + launcher.setBuffer(m_sum.getBufferCL()); + launcher.setBuffer(m_sum2.getBufferCL()); + launcher.setConst( numSmallAabbs ); + int num = numSmallAabbs; + launcher.launch1D( num); + + + b3Vector3 s; + b3Vector3 s2; + m_prefixScanFloat4->execute(m_sum,m_dst,numSmallAabbs+1,&s); + m_prefixScanFloat4->execute(m_sum2,m_dst,numSmallAabbs+1,&s2); + + b3Vector3 v = s2 - (s*s) / (float)numSmallAabbs; + + if(v[1] > v[0]) + axis = 1; + if(v[2] > v[axis]) + axis = 2; + } + + + + m_gpuSmallSortData.resize(numSmallAabbs); + + +#if 1 + if (m_smallAabbsMappingGPU.size()) + { + + B3_PROFILE("flipFloatKernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( m_allAabbsGPU.getBufferCL(), true ), + b3BufferInfoCL( m_smallAabbsMappingGPU.getBufferCL(), true), + b3BufferInfoCL( m_gpuSmallSortData.getBufferCL())}; + b3LauncherCL launcher(m_queue, m_flipFloatKernel ,"m_flipFloatKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( numSmallAabbs ); + launcher.setConst( axis ); + + int num = numSmallAabbs; + launcher.launch1D( num); + clFinish(m_queue); + } + + if (m_gpuSmallSortData.size()) + { + B3_PROFILE("gpu radix sort"); + m_sorter->execute(m_gpuSmallSortData); + clFinish(m_queue); + } + + m_gpuSmallSortedAabbs.resize(numSmallAabbs); + if (numSmallAabbs) + { + B3_PROFILE("scatterKernel"); + + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( m_allAabbsGPU.getBufferCL(), true ), + b3BufferInfoCL( m_smallAabbsMappingGPU.getBufferCL(), true), + b3BufferInfoCL( m_gpuSmallSortData.getBufferCL(),true), + b3BufferInfoCL(m_gpuSmallSortedAabbs.getBufferCL())}; + b3LauncherCL launcher(m_queue, m_scatterKernel ,"m_scatterKernel "); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( numSmallAabbs); + int num = numSmallAabbs; + launcher.launch1D( num); + clFinish(m_queue); + + } + + + m_overlappingPairs.resize(maxPairs); + + m_pairCount.resize(0); + m_pairCount.push_back(0); + int numPairs=0; + + { + int numLargeAabbs = m_largeAabbsMappingGPU.size(); + if (numLargeAabbs && numSmallAabbs) + { + //@todo + B3_PROFILE("sap2Kernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( m_allAabbsGPU.getBufferCL() ), + b3BufferInfoCL( m_largeAabbsMappingGPU.getBufferCL() ), + b3BufferInfoCL( m_smallAabbsMappingGPU.getBufferCL() ), + b3BufferInfoCL( m_overlappingPairs.getBufferCL() ), + b3BufferInfoCL(m_pairCount.getBufferCL())}; + b3LauncherCL launcher(m_queue, m_sap2Kernel,"m_sap2Kernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( numLargeAabbs ); + launcher.setConst( numSmallAabbs); + launcher.setConst( axis ); + launcher.setConst( maxPairs ); +//@todo: use actual maximum work item sizes of the device instead of hardcoded values + launcher.launch2D( numLargeAabbs, numSmallAabbs,4,64); + + numPairs = m_pairCount.at(0); + if (numPairs >maxPairs) + { + b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs); + numPairs =maxPairs; + } + } + } + if (m_gpuSmallSortedAabbs.size()) + { + B3_PROFILE("sapKernel"); + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_gpuSmallSortedAabbs.getBufferCL() ), b3BufferInfoCL( m_overlappingPairs.getBufferCL() ), b3BufferInfoCL(m_pairCount.getBufferCL())}; + b3LauncherCL launcher(m_queue, m_sapKernel,"m_sapKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( numSmallAabbs ); + launcher.setConst( axis ); + launcher.setConst( maxPairs ); + + + int num = numSmallAabbs; +#if 0 + int buffSize = launcher.getSerializationBufferSize(); + unsigned char* buf = new unsigned char[buffSize+sizeof(int)]; + for (int i=0;imaxPairs) + { + b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs); + numPairs = maxPairs; + m_pairCount.resize(0); + m_pairCount.push_back(maxPairs); + } + } + +#else + int numPairs = 0; + + + b3LauncherCL launcher(m_queue, m_sapKernel); + + const char* fileName = "m_sapKernelArgs.bin"; + FILE* f = fopen(fileName,"rb"); + if (f) + { + int sizeInBytes=0; + if (fseek(f, 0, SEEK_END) || (sizeInBytes = ftell(f)) == EOF || fseek(f, 0, SEEK_SET)) + { + printf("error, cannot get file size\n"); + exit(0); + } + + unsigned char* buf = (unsigned char*) malloc(sizeInBytes); + fread(buf,sizeInBytes,1,f); + int serializedBytes = launcher.deserializeArgs(buf, sizeInBytes,m_context); + int num = *(int*)&buf[serializedBytes]; + launcher.launch1D( num); + + b3OpenCLArray pairCount(m_context, m_queue); + int numElements = launcher.m_arrays[2]->size()/sizeof(int); + pairCount.setFromOpenCLBuffer(launcher.m_arrays[2]->getBufferCL(),numElements); + numPairs = pairCount.at(0); + //printf("overlapping pairs = %d\n",numPairs); + b3AlignedObjectArray hostOoverlappingPairs; + b3OpenCLArray tmpGpuPairs(m_context,m_queue); + tmpGpuPairs.setFromOpenCLBuffer(launcher.m_arrays[1]->getBufferCL(),numPairs ); + + tmpGpuPairs.copyToHost(hostOoverlappingPairs); + m_overlappingPairs.copyFromHost(hostOoverlappingPairs); + //printf("hello %d\n", m_overlappingPairs.size()); + free(buf); + fclose(f); + + } else { + printf("error: cannot find file %s\n",fileName); + } + + clFinish(m_queue); + + +#endif + + + m_overlappingPairs.resize(numPairs); + + }//B3_PROFILE("GPU_RADIX SORT"); + //init3dSap(); +} + +void b3GpuSapBroadphase::writeAabbsToGpu() +{ + m_smallAabbsMappingGPU.copyFromHost(m_smallAabbsMappingCPU); + m_largeAabbsMappingGPU.copyFromHost(m_largeAabbsMappingCPU); + + m_allAabbsGPU.copyFromHost(m_allAabbsCPU);//might not be necessary, the 'setupGpuAabbsFull' already takes care of this + + + +} + +void b3GpuSapBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask) +{ + int index = userPtr; + b3SapAabb aabb; + for (int i=0;i<4;i++) + { + aabb.m_min[i] = aabbMin[i]; + aabb.m_max[i] = aabbMax[i]; + } + aabb.m_minIndices[3] = index; + aabb.m_signedMaxIndices[3] = m_allAabbsCPU.size(); + m_largeAabbsMappingCPU.push_back(m_allAabbsCPU.size()); + + m_allAabbsCPU.push_back(aabb); +} + +void b3GpuSapBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask) +{ + int index = userPtr; + b3SapAabb aabb; + for (int i=0;i<4;i++) + { + aabb.m_min[i] = aabbMin[i]; + aabb.m_max[i] = aabbMax[i]; + } + aabb.m_minIndices[3] = index; + aabb.m_signedMaxIndices[3] = m_allAabbsCPU.size(); + m_smallAabbsMappingCPU.push_back(m_allAabbsCPU.size()); + + + m_allAabbsCPU.push_back(aabb); +} + +cl_mem b3GpuSapBroadphase::getAabbBufferWS() +{ + return m_allAabbsGPU.getBufferCL(); +} + +int b3GpuSapBroadphase::getNumOverlap() +{ + return m_overlappingPairs.size(); +} +cl_mem b3GpuSapBroadphase::getOverlappingPairBuffer() +{ + return m_overlappingPairs.getBufferCL(); +} + +b3OpenCLArray& b3GpuSapBroadphase::getOverlappingPairsGPU() +{ + return m_overlappingPairs; +} +b3OpenCLArray& b3GpuSapBroadphase::getSmallAabbIndicesGPU() +{ + return m_smallAabbsMappingGPU; +} +b3OpenCLArray& b3GpuSapBroadphase::getLargeAabbIndicesGPU() +{ + return m_largeAabbsMappingGPU; +} diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h new file mode 100644 index 000000000000..8d36ac78f28c --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h @@ -0,0 +1,151 @@ +#ifndef B3_GPU_SAP_BROADPHASE_H +#define B3_GPU_SAP_BROADPHASE_H + +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3FillCL.h" //b3Int2 +class b3Vector3; +#include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h" + +#include "b3SapAabb.h" +#include "Bullet3Common/shared/b3Int2.h" + +#include "b3GpuBroadphaseInterface.h" + + +class b3GpuSapBroadphase : public b3GpuBroadphaseInterface +{ + + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; + cl_kernel m_flipFloatKernel; + cl_kernel m_scatterKernel ; + cl_kernel m_copyAabbsKernel; + cl_kernel m_sapKernel; + cl_kernel m_sap2Kernel; + cl_kernel m_prepareSumVarianceKernel; + + + class b3RadixSort32CL* m_sorter; + + ///test for 3d SAP + b3AlignedObjectArray m_sortedAxisCPU[3][2]; + b3AlignedObjectArray m_objectMinMaxIndexCPU[3][2]; + b3OpenCLArray m_objectMinMaxIndexGPUaxis0; + b3OpenCLArray m_objectMinMaxIndexGPUaxis1; + b3OpenCLArray m_objectMinMaxIndexGPUaxis2; + b3OpenCLArray m_objectMinMaxIndexGPUaxis0prev; + b3OpenCLArray m_objectMinMaxIndexGPUaxis1prev; + b3OpenCLArray m_objectMinMaxIndexGPUaxis2prev; + + b3OpenCLArray m_sortedAxisGPU0; + b3OpenCLArray m_sortedAxisGPU1; + b3OpenCLArray m_sortedAxisGPU2; + b3OpenCLArray m_sortedAxisGPU0prev; + b3OpenCLArray m_sortedAxisGPU1prev; + b3OpenCLArray m_sortedAxisGPU2prev; + + + b3OpenCLArray m_addedHostPairsGPU; + b3OpenCLArray m_removedHostPairsGPU; + b3OpenCLArray m_addedCountGPU; + b3OpenCLArray m_removedCountGPU; + + int m_currentBuffer; + +public: + + b3OpenCLArray m_pairCount; + + + b3OpenCLArray m_allAabbsGPU; + b3AlignedObjectArray m_allAabbsCPU; + + virtual b3OpenCLArray& getAllAabbsGPU() + { + return m_allAabbsGPU; + } + virtual b3AlignedObjectArray& getAllAabbsCPU() + { + return m_allAabbsCPU; + } + + b3OpenCLArray m_sum; + b3OpenCLArray m_sum2; + b3OpenCLArray m_dst; + + b3OpenCLArray m_smallAabbsMappingGPU; + b3AlignedObjectArray m_smallAabbsMappingCPU; + + b3OpenCLArray m_largeAabbsMappingGPU; + b3AlignedObjectArray m_largeAabbsMappingCPU; + + + b3OpenCLArray m_overlappingPairs; + + //temporary gpu work memory + b3OpenCLArray m_gpuSmallSortData; + b3OpenCLArray m_gpuSmallSortedAabbs; + + class b3PrefixScanFloat4CL* m_prefixScanFloat4; + + enum b3GpuSapKernelType + { + B3_GPU_SAP_KERNEL_BRUTE_FORCE_CPU=1, + B3_GPU_SAP_KERNEL_BRUTE_FORCE_GPU, + B3_GPU_SAP_KERNEL_ORIGINAL, + B3_GPU_SAP_KERNEL_BARRIER, + B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY + }; + + b3GpuSapBroadphase(cl_context ctx,cl_device_id device, cl_command_queue q , b3GpuSapKernelType kernelType=B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY); + virtual ~b3GpuSapBroadphase(); + + static b3GpuBroadphaseInterface* CreateFuncBruteForceCpu(cl_context ctx,cl_device_id device, cl_command_queue q) + { + return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_BRUTE_FORCE_CPU); + } + + static b3GpuBroadphaseInterface* CreateFuncBruteForceGpu(cl_context ctx,cl_device_id device, cl_command_queue q) + { + return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_BRUTE_FORCE_GPU); + } + + static b3GpuBroadphaseInterface* CreateFuncOriginal(cl_context ctx,cl_device_id device, cl_command_queue q) + { + return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_ORIGINAL); + } + static b3GpuBroadphaseInterface* CreateFuncBarrier(cl_context ctx,cl_device_id device, cl_command_queue q) + { + return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_BARRIER); + } + static b3GpuBroadphaseInterface* CreateFuncLocalMemory(cl_context ctx,cl_device_id device, cl_command_queue q) + { + return new b3GpuSapBroadphase(ctx,device,q,B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY); + } + + + virtual void calculateOverlappingPairs(int maxPairs); + virtual void calculateOverlappingPairsHost(int maxPairs); + + void reset(); + + void init3dSap(); + virtual void calculateOverlappingPairsHostIncremental3Sap(); + + virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask); + virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr , int collisionFilterGroup, int collisionFilterMask); + + //call writeAabbsToGpu after done making all changes (createProxy etc) + virtual void writeAabbsToGpu(); + + virtual cl_mem getAabbBufferWS(); + virtual int getNumOverlap(); + virtual cl_mem getOverlappingPairBuffer(); + + virtual b3OpenCLArray& getOverlappingPairsGPU(); + virtual b3OpenCLArray& getSmallAabbIndicesGPU(); + virtual b3OpenCLArray& getLargeAabbIndicesGPU(); +}; + +#endif //B3_GPU_SAP_BROADPHASE_H \ No newline at end of file diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h new file mode 100644 index 000000000000..ea6550fedea5 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h @@ -0,0 +1,14 @@ +#ifndef B3_SAP_AABB_H +#define B3_SAP_AABB_H + +#include "Bullet3Common/b3Scalar.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" + +///just make sure that the b3Aabb is 16-byte aligned +B3_ATTRIBUTE_ALIGNED16(struct) b3SapAabb : public b3Aabb +{ + +}; + + +#endif //B3_SAP_AABB_H diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphase.cl b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphase.cl new file mode 100644 index 000000000000..ded4796d337d --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphase.cl @@ -0,0 +1,216 @@ + + +int getPosHash(int4 gridPos, __global float4* pParams) +{ + int4 gridDim = *((__global int4*)(pParams + 1)); + gridPos.x &= gridDim.x - 1; + gridPos.y &= gridDim.y - 1; + gridPos.z &= gridDim.z - 1; + int hash = gridPos.z * gridDim.y * gridDim.x + gridPos.y * gridDim.x + gridPos.x; + return hash; +} + +int4 getGridPos(float4 worldPos, __global float4* pParams) +{ + int4 gridPos; + int4 gridDim = *((__global int4*)(pParams + 1)); + gridPos.x = (int)floor(worldPos.x * pParams[0].x) & (gridDim.x - 1); + gridPos.y = (int)floor(worldPos.y * pParams[0].y) & (gridDim.y - 1); + gridPos.z = (int)floor(worldPos.z * pParams[0].z) & (gridDim.z - 1); + return gridPos; +} + + +// calculate grid hash value for each body using its AABB +__kernel void kCalcHashAABB(int numObjects, __global float4* allpAABB, __global const int* smallAabbMapping, __global int2* pHash, __global float4* pParams ) +{ + int index = get_global_id(0); + if(index >= numObjects) + { + return; + } + float4 bbMin = allpAABB[smallAabbMapping[index]*2]; + float4 bbMax = allpAABB[smallAabbMapping[index]*2 + 1]; + float4 pos; + pos.x = (bbMin.x + bbMax.x) * 0.5f; + pos.y = (bbMin.y + bbMax.y) * 0.5f; + pos.z = (bbMin.z + bbMax.z) * 0.5f; + pos.w = 0.f; + // get address in grid + int4 gridPos = getGridPos(pos, pParams); + int gridHash = getPosHash(gridPos, pParams); + // store grid hash and body index + int2 hashVal; + hashVal.x = gridHash; + hashVal.y = index; + pHash[index] = hashVal; +} + +__kernel void kClearCellStart( int numCells, + __global int* pCellStart ) +{ + int index = get_global_id(0); + if(index >= numCells) + { + return; + } + pCellStart[index] = -1; +} + +__kernel void kFindCellStart(int numObjects, __global int2* pHash, __global int* cellStart ) +{ + __local int sharedHash[513]; + int index = get_global_id(0); + int2 sortedData; + + if(index < numObjects) + { + sortedData = pHash[index]; + // Load hash data into shared memory so that we can look + // at neighboring body's hash value without loading + // two hash values per thread + sharedHash[get_local_id(0) + 1] = sortedData.x; + if((index > 0) && (get_local_id(0) == 0)) + { + // first thread in block must load neighbor body hash + sharedHash[0] = pHash[index-1].x; + } + } + barrier(CLK_LOCAL_MEM_FENCE); + if(index < numObjects) + { + if((index == 0) || (sortedData.x != sharedHash[get_local_id(0)])) + { + cellStart[sortedData.x] = index; + } + } +} + +int testAABBOverlap(float4 min0, float4 max0, float4 min1, float4 max1) +{ + return (min0.x <= max1.x)&& (min1.x <= max0.x) && + (min0.y <= max1.y)&& (min1.y <= max0.y) && + (min0.z <= max1.z)&& (min1.z <= max0.z); +} + + + + +//search for AABB 'index' against other AABBs' in this cell +void findPairsInCell( int numObjects, + int4 gridPos, + int index, + __global int2* pHash, + __global int* pCellStart, + __global float4* allpAABB, + __global const int* smallAabbMapping, + __global float4* pParams, + volatile __global int* pairCount, + __global int4* pPairBuff2, + int maxPairs + ) +{ + int4 pGridDim = *((__global int4*)(pParams + 1)); + int maxBodiesPerCell = pGridDim.w; + int gridHash = getPosHash(gridPos, pParams); + // get start of bucket for this cell + int bucketStart = pCellStart[gridHash]; + if (bucketStart == -1) + { + return; // cell empty + } + // iterate over bodies in this cell + int2 sortedData = pHash[index]; + int unsorted_indx = sortedData.y; + float4 min0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0]; + float4 max0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1]; + int handleIndex = as_int(min0.w); + + int bucketEnd = bucketStart + maxBodiesPerCell; + bucketEnd = (bucketEnd > numObjects) ? numObjects : bucketEnd; + for(int index2 = bucketStart; index2 < bucketEnd; index2++) + { + int2 cellData = pHash[index2]; + if (cellData.x != gridHash) + { + break; // no longer in same bucket + } + int unsorted_indx2 = cellData.y; + //if (unsorted_indx2 < unsorted_indx) // check not colliding with self + if (unsorted_indx2 != unsorted_indx) // check not colliding with self + { + float4 min1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 0]; + float4 max1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 1]; + if(testAABBOverlap(min0, max0, min1, max1)) + { + if (pairCount) + { + int handleIndex2 = as_int(min1.w); + if (handleIndex= numObjects) + { + return; + } + int2 sortedData = pHash[index]; + int unsorted_indx = sortedData.y; + float4 bbMin = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0]; + float4 bbMax = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1]; + float4 pos; + pos.x = (bbMin.x + bbMax.x) * 0.5f; + pos.y = (bbMin.y + bbMax.y) * 0.5f; + pos.z = (bbMin.z + bbMax.z) * 0.5f; + // get address in grid + int4 gridPosA = getGridPos(pos, pParams); + int4 gridPosB; + // examine only neighbouring cells + for(int z=-1; z<=1; z++) + { + gridPosB.z = gridPosA.z + z; + for(int y=-1; y<=1; y++) + { + gridPosB.y = gridPosA.y + y; + for(int x=-1; x<=1; x++) + { + gridPosB.x = gridPosA.x + x; + findPairsInCell(numObjects, gridPosB, index, pHash, pCellStart, allpAABB,smallAabbMapping, pParams, pairCount,pPairBuff2, maxPairs); + } + } + } +} + + + + + diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphaseKernels.h b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphaseKernels.h new file mode 100644 index 000000000000..dad42477c376 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphaseKernels.h @@ -0,0 +1,199 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* gridBroadphaseCL= \ +"int getPosHash(int4 gridPos, __global float4* pParams)\n" +"{\n" +" int4 gridDim = *((__global int4*)(pParams + 1));\n" +" gridPos.x &= gridDim.x - 1;\n" +" gridPos.y &= gridDim.y - 1;\n" +" gridPos.z &= gridDim.z - 1;\n" +" int hash = gridPos.z * gridDim.y * gridDim.x + gridPos.y * gridDim.x + gridPos.x;\n" +" return hash;\n" +"} \n" +"int4 getGridPos(float4 worldPos, __global float4* pParams)\n" +"{\n" +" int4 gridPos;\n" +" int4 gridDim = *((__global int4*)(pParams + 1));\n" +" gridPos.x = (int)floor(worldPos.x * pParams[0].x) & (gridDim.x - 1);\n" +" gridPos.y = (int)floor(worldPos.y * pParams[0].y) & (gridDim.y - 1);\n" +" gridPos.z = (int)floor(worldPos.z * pParams[0].z) & (gridDim.z - 1);\n" +" return gridPos;\n" +"}\n" +"// calculate grid hash value for each body using its AABB\n" +"__kernel void kCalcHashAABB(int numObjects, __global float4* allpAABB, __global const int* smallAabbMapping, __global int2* pHash, __global float4* pParams )\n" +"{\n" +" int index = get_global_id(0);\n" +" if(index >= numObjects)\n" +" {\n" +" return;\n" +" }\n" +" float4 bbMin = allpAABB[smallAabbMapping[index]*2];\n" +" float4 bbMax = allpAABB[smallAabbMapping[index]*2 + 1];\n" +" float4 pos;\n" +" pos.x = (bbMin.x + bbMax.x) * 0.5f;\n" +" pos.y = (bbMin.y + bbMax.y) * 0.5f;\n" +" pos.z = (bbMin.z + bbMax.z) * 0.5f;\n" +" pos.w = 0.f;\n" +" // get address in grid\n" +" int4 gridPos = getGridPos(pos, pParams);\n" +" int gridHash = getPosHash(gridPos, pParams);\n" +" // store grid hash and body index\n" +" int2 hashVal;\n" +" hashVal.x = gridHash;\n" +" hashVal.y = index;\n" +" pHash[index] = hashVal;\n" +"}\n" +"__kernel void kClearCellStart( int numCells, \n" +" __global int* pCellStart )\n" +"{\n" +" int index = get_global_id(0);\n" +" if(index >= numCells)\n" +" {\n" +" return;\n" +" }\n" +" pCellStart[index] = -1;\n" +"}\n" +"__kernel void kFindCellStart(int numObjects, __global int2* pHash, __global int* cellStart )\n" +"{\n" +" __local int sharedHash[513];\n" +" int index = get_global_id(0);\n" +" int2 sortedData;\n" +" if(index < numObjects)\n" +" {\n" +" sortedData = pHash[index];\n" +" // Load hash data into shared memory so that we can look \n" +" // at neighboring body's hash value without loading\n" +" // two hash values per thread\n" +" sharedHash[get_local_id(0) + 1] = sortedData.x;\n" +" if((index > 0) && (get_local_id(0) == 0))\n" +" {\n" +" // first thread in block must load neighbor body hash\n" +" sharedHash[0] = pHash[index-1].x;\n" +" }\n" +" }\n" +" barrier(CLK_LOCAL_MEM_FENCE);\n" +" if(index < numObjects)\n" +" {\n" +" if((index == 0) || (sortedData.x != sharedHash[get_local_id(0)]))\n" +" {\n" +" cellStart[sortedData.x] = index;\n" +" }\n" +" }\n" +"}\n" +"int testAABBOverlap(float4 min0, float4 max0, float4 min1, float4 max1)\n" +"{\n" +" return (min0.x <= max1.x)&& (min1.x <= max0.x) && \n" +" (min0.y <= max1.y)&& (min1.y <= max0.y) && \n" +" (min0.z <= max1.z)&& (min1.z <= max0.z); \n" +"}\n" +"//search for AABB 'index' against other AABBs' in this cell\n" +"void findPairsInCell( int numObjects,\n" +" int4 gridPos,\n" +" int index,\n" +" __global int2* pHash,\n" +" __global int* pCellStart,\n" +" __global float4* allpAABB, \n" +" __global const int* smallAabbMapping,\n" +" __global float4* pParams,\n" +" volatile __global int* pairCount,\n" +" __global int4* pPairBuff2,\n" +" int maxPairs\n" +" )\n" +"{\n" +" int4 pGridDim = *((__global int4*)(pParams + 1));\n" +" int maxBodiesPerCell = pGridDim.w;\n" +" int gridHash = getPosHash(gridPos, pParams);\n" +" // get start of bucket for this cell\n" +" int bucketStart = pCellStart[gridHash];\n" +" if (bucketStart == -1)\n" +" {\n" +" return; // cell empty\n" +" }\n" +" // iterate over bodies in this cell\n" +" int2 sortedData = pHash[index];\n" +" int unsorted_indx = sortedData.y;\n" +" float4 min0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0]; \n" +" float4 max0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1];\n" +" int handleIndex = as_int(min0.w);\n" +" \n" +" int bucketEnd = bucketStart + maxBodiesPerCell;\n" +" bucketEnd = (bucketEnd > numObjects) ? numObjects : bucketEnd;\n" +" for(int index2 = bucketStart; index2 < bucketEnd; index2++) \n" +" {\n" +" int2 cellData = pHash[index2];\n" +" if (cellData.x != gridHash)\n" +" {\n" +" break; // no longer in same bucket\n" +" }\n" +" int unsorted_indx2 = cellData.y;\n" +" //if (unsorted_indx2 < unsorted_indx) // check not colliding with self\n" +" if (unsorted_indx2 != unsorted_indx) // check not colliding with self\n" +" { \n" +" float4 min1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 0];\n" +" float4 max1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 1];\n" +" if(testAABBOverlap(min0, max0, min1, max1))\n" +" {\n" +" if (pairCount)\n" +" {\n" +" int handleIndex2 = as_int(min1.w);\n" +" if (handleIndex= numObjects)\n" +" {\n" +" return;\n" +" }\n" +" int2 sortedData = pHash[index];\n" +" int unsorted_indx = sortedData.y;\n" +" float4 bbMin = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0];\n" +" float4 bbMax = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1];\n" +" float4 pos;\n" +" pos.x = (bbMin.x + bbMax.x) * 0.5f;\n" +" pos.y = (bbMin.y + bbMax.y) * 0.5f;\n" +" pos.z = (bbMin.z + bbMax.z) * 0.5f;\n" +" // get address in grid\n" +" int4 gridPosA = getGridPos(pos, pParams);\n" +" int4 gridPosB; \n" +" // examine only neighbouring cells\n" +" for(int z=-1; z<=1; z++) \n" +" {\n" +" gridPosB.z = gridPosA.z + z;\n" +" for(int y=-1; y<=1; y++) \n" +" {\n" +" gridPosB.y = gridPosA.y + y;\n" +" for(int x=-1; x<=1; x++) \n" +" {\n" +" gridPosB.x = gridPosA.x + x;\n" +" findPairsInCell(numObjects, gridPosB, index, pHash, pCellStart, allpAABB,smallAabbMapping, pParams, pairCount,pPairBuff2, maxPairs);\n" +" }\n" +" }\n" +" }\n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvh.cl b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvh.cl new file mode 100644 index 000000000000..c375b9bf37e5 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvh.cl @@ -0,0 +1,767 @@ +/* +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. +*/ +//Initial Author Jackson Lee, 2014 + +typedef float b3Scalar; +typedef float4 b3Vector3; +#define b3Max max +#define b3Min min +#define b3Sqrt sqrt + +typedef struct +{ + unsigned int m_key; + unsigned int m_value; +} SortDataCL; + +typedef struct +{ + union + { + float4 m_min; + float m_minElems[4]; + int m_minIndices[4]; + }; + union + { + float4 m_max; + float m_maxElems[4]; + int m_maxIndices[4]; + }; +} b3AabbCL; + + +unsigned int interleaveBits(unsigned int x) +{ + //........ ........ ......12 3456789A //x + //....1..2 ..3..4.. 5..6..7. .8..9..A //x after interleaving bits + + //......12 3456789A ......12 3456789A //x ^ (x << 16) + //11111111 ........ ........ 11111111 //0x FF 00 00 FF + //......12 ........ ........ 3456789A //x = (x ^ (x << 16)) & 0xFF0000FF; + + //......12 ........ 3456789A 3456789A //x ^ (x << 8) + //......11 ........ 1111.... ....1111 //0x 03 00 F0 0F + //......12 ........ 3456.... ....789A //x = (x ^ (x << 8)) & 0x0300F00F; + + //..12..12 ....3456 3456.... 789A789A //x ^ (x << 4) + //......11 ....11.. ..11.... 11....11 //0x 03 0C 30 C3 + //......12 ....34.. ..56.... 78....9A //x = (x ^ (x << 4)) & 0x030C30C3; + + //....1212 ..3434.. 5656..78 78..9A9A //x ^ (x << 2) + //....1..1 ..1..1.. 1..1..1. .1..1..1 //0x 09 24 92 49 + //....1..2 ..3..4.. 5..6..7. .8..9..A //x = (x ^ (x << 2)) & 0x09249249; + + //........ ........ ......11 11111111 //0x000003FF + x &= 0x000003FF; //Clear all bits above bit 10 + + x = (x ^ (x << 16)) & 0xFF0000FF; + x = (x ^ (x << 8)) & 0x0300F00F; + x = (x ^ (x << 4)) & 0x030C30C3; + x = (x ^ (x << 2)) & 0x09249249; + + return x; +} +unsigned int getMortonCode(unsigned int x, unsigned int y, unsigned int z) +{ + return interleaveBits(x) << 0 | interleaveBits(y) << 1 | interleaveBits(z) << 2; +} + +__kernel void separateAabbs(__global b3AabbCL* unseparatedAabbs, __global int* aabbIndices, __global b3AabbCL* out_aabbs, int numAabbsToSeparate) +{ + int separatedAabbIndex = get_global_id(0); + if(separatedAabbIndex >= numAabbsToSeparate) return; + + int unseparatedAabbIndex = aabbIndices[separatedAabbIndex]; + out_aabbs[separatedAabbIndex] = unseparatedAabbs[unseparatedAabbIndex]; +} + +//Should replace with an optimized parallel reduction +__kernel void findAllNodesMergedAabb(__global b3AabbCL* out_mergedAabb, int numAabbsNeedingMerge) +{ + //Each time this kernel is added to the command queue, + //the number of AABBs needing to be merged is halved + // + //Example with 159 AABBs: + // numRemainingAabbs == 159 / 2 + 159 % 2 == 80 + // numMergedAabbs == 159 - 80 == 79 + //So, indices [0, 78] are merged with [0 + 80, 78 + 80] + + int numRemainingAabbs = numAabbsNeedingMerge / 2 + numAabbsNeedingMerge % 2; + int numMergedAabbs = numAabbsNeedingMerge - numRemainingAabbs; + + int aabbIndex = get_global_id(0); + if(aabbIndex >= numMergedAabbs) return; + + int otherAabbIndex = aabbIndex + numRemainingAabbs; + + b3AabbCL aabb = out_mergedAabb[aabbIndex]; + b3AabbCL otherAabb = out_mergedAabb[otherAabbIndex]; + + b3AabbCL mergedAabb; + mergedAabb.m_min = b3Min(aabb.m_min, otherAabb.m_min); + mergedAabb.m_max = b3Max(aabb.m_max, otherAabb.m_max); + out_mergedAabb[aabbIndex] = mergedAabb; +} + +__kernel void assignMortonCodesAndAabbIndicies(__global b3AabbCL* worldSpaceAabbs, __global b3AabbCL* mergedAabbOfAllNodes, + __global SortDataCL* out_mortonCodesAndAabbIndices, int numAabbs) +{ + int leafNodeIndex = get_global_id(0); //Leaf node index == AABB index + if(leafNodeIndex >= numAabbs) return; + + b3AabbCL mergedAabb = mergedAabbOfAllNodes[0]; + b3Vector3 gridCenter = (mergedAabb.m_min + mergedAabb.m_max) * 0.5f; + b3Vector3 gridCellSize = (mergedAabb.m_max - mergedAabb.m_min) / (float)1024; + + b3AabbCL aabb = worldSpaceAabbs[leafNodeIndex]; + b3Vector3 aabbCenter = (aabb.m_min + aabb.m_max) * 0.5f; + b3Vector3 aabbCenterRelativeToGrid = aabbCenter - gridCenter; + + //Quantize into integer coordinates + //floor() is needed to prevent the center cell, at (0,0,0) from being twice the size + b3Vector3 gridPosition = aabbCenterRelativeToGrid / gridCellSize; + + int4 discretePosition; + discretePosition.x = (int)( (gridPosition.x >= 0.0f) ? gridPosition.x : floor(gridPosition.x) ); + discretePosition.y = (int)( (gridPosition.y >= 0.0f) ? gridPosition.y : floor(gridPosition.y) ); + discretePosition.z = (int)( (gridPosition.z >= 0.0f) ? gridPosition.z : floor(gridPosition.z) ); + + //Clamp coordinates into [-512, 511], then convert range from [-512, 511] to [0, 1023] + discretePosition = b3Max( -512, b3Min(discretePosition, 511) ); + discretePosition += 512; + + //Interleave bits(assign a morton code, also known as a z-curve) + unsigned int mortonCode = getMortonCode(discretePosition.x, discretePosition.y, discretePosition.z); + + // + SortDataCL mortonCodeIndexPair; + mortonCodeIndexPair.m_key = mortonCode; + mortonCodeIndexPair.m_value = leafNodeIndex; + + out_mortonCodesAndAabbIndices[leafNodeIndex] = mortonCodeIndexPair; +} + +#define B3_PLVBH_TRAVERSE_MAX_STACK_SIZE 128 + +//The most significant bit(0x80000000) of a int32 is used to distinguish between leaf and internal nodes. +//If it is set, then the index is for an internal node; otherwise, it is a leaf node. +//In both cases, the bit should be cleared to access the actual node index. +int isLeafNode(int index) { return (index >> 31 == 0); } +int getIndexWithInternalNodeMarkerRemoved(int index) { return index & (~0x80000000); } +int getIndexWithInternalNodeMarkerSet(int isLeaf, int index) { return (isLeaf) ? index : (index | 0x80000000); } + +//From sap.cl +#define NEW_PAIR_MARKER -1 + +bool TestAabbAgainstAabb2(const b3AabbCL* aabb1, const b3AabbCL* aabb2) +{ + bool overlap = true; + overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap; + overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap; + overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap; + return overlap; +} +//From sap.cl + +__kernel void plbvhCalculateOverlappingPairs(__global b3AabbCL* rigidAabbs, + + __global int* rootNodeIndex, + __global int2* internalNodeChildIndices, + __global b3AabbCL* internalNodeAabbs, + __global int2* internalNodeLeafIndexRanges, + + __global SortDataCL* mortonCodesAndAabbIndices, + __global int* out_numPairs, __global int4* out_overlappingPairs, + int maxPairs, int numQueryAabbs) +{ + //Using get_group_id()/get_local_id() is Faster than get_global_id(0) since + //mortonCodesAndAabbIndices[] contains rigid body indices sorted along the z-curve (more spatially coherent) + int queryBvhNodeIndex = get_group_id(0) * get_local_size(0) + get_local_id(0); + if(queryBvhNodeIndex >= numQueryAabbs) return; + + int queryRigidIndex = mortonCodesAndAabbIndices[queryBvhNodeIndex].m_value; + b3AabbCL queryAabb = rigidAabbs[queryRigidIndex]; + + int stack[B3_PLVBH_TRAVERSE_MAX_STACK_SIZE]; + + int stackSize = 1; + stack[0] = *rootNodeIndex; + + while(stackSize) + { + int internalOrLeafNodeIndex = stack[ stackSize - 1 ]; + --stackSize; + + int isLeaf = isLeafNode(internalOrLeafNodeIndex); //Internal node if false + int bvhNodeIndex = getIndexWithInternalNodeMarkerRemoved(internalOrLeafNodeIndex); + + //Optimization - if the BVH is structured as a binary radix tree, then + //each internal node corresponds to a contiguous range of leaf nodes(internalNodeLeafIndexRanges[]). + //This can be used to avoid testing each AABB-AABB pair twice, including preventing each node from colliding with itself. + { + int highestLeafIndex = (isLeaf) ? bvhNodeIndex : internalNodeLeafIndexRanges[bvhNodeIndex].y; + if(highestLeafIndex <= queryBvhNodeIndex) continue; + } + + //bvhRigidIndex is not used if internal node + int bvhRigidIndex = (isLeaf) ? mortonCodesAndAabbIndices[bvhNodeIndex].m_value : -1; + + b3AabbCL bvhNodeAabb = (isLeaf) ? rigidAabbs[bvhRigidIndex] : internalNodeAabbs[bvhNodeIndex]; + if( TestAabbAgainstAabb2(&queryAabb, &bvhNodeAabb) ) + { + if(isLeaf) + { + int4 pair; + pair.x = rigidAabbs[queryRigidIndex].m_minIndices[3]; + pair.y = rigidAabbs[bvhRigidIndex].m_minIndices[3]; + pair.z = NEW_PAIR_MARKER; + pair.w = NEW_PAIR_MARKER; + + int pairIndex = atomic_inc(out_numPairs); + if(pairIndex < maxPairs) out_overlappingPairs[pairIndex] = pair; + } + + if(!isLeaf) //Internal node + { + if(stackSize + 2 > B3_PLVBH_TRAVERSE_MAX_STACK_SIZE) + { + //Error + } + else + { + stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].x; + stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].y; + } + } + } + + } +} + + +//From rayCastKernels.cl +typedef struct +{ + float4 m_from; + float4 m_to; +} b3RayInfo; +//From rayCastKernels.cl + +b3Vector3 b3Vector3_normalize(b3Vector3 v) +{ + b3Vector3 normal = (b3Vector3){v.x, v.y, v.z, 0.f}; + return normalize(normal); //OpenCL normalize == vector4 normalize +} +b3Scalar b3Vector3_length2(b3Vector3 v) { return v.x*v.x + v.y*v.y + v.z*v.z; } +b3Scalar b3Vector3_dot(b3Vector3 a, b3Vector3 b) { return a.x*b.x + a.y*b.y + a.z*b.z; } + +int rayIntersectsAabb(b3Vector3 rayOrigin, b3Scalar rayLength, b3Vector3 rayNormalizedDirection, b3AabbCL aabb) +{ + //AABB is considered as 3 pairs of 2 planes( {x_min, x_max}, {y_min, y_max}, {z_min, z_max} ). + //t_min is the point of intersection with the closer plane, t_max is the point of intersection with the farther plane. + // + //if (rayNormalizedDirection.x < 0.0f), then max.x will be the near plane + //and min.x will be the far plane; otherwise, it is reversed. + // + //In order for there to be a collision, the t_min and t_max of each pair must overlap. + //This can be tested for by selecting the highest t_min and lowest t_max and comparing them. + + int4 isNegative = isless( rayNormalizedDirection, ((b3Vector3){0.0f, 0.0f, 0.0f, 0.0f}) ); //isless(x,y) returns (x < y) + + //When using vector types, the select() function checks the most signficant bit, + //but isless() sets the least significant bit. + isNegative <<= 31; + + //select(b, a, condition) == condition ? a : b + //When using select() with vector types, (condition[i]) is true if its most significant bit is 1 + b3Vector3 t_min = ( select(aabb.m_min, aabb.m_max, isNegative) - rayOrigin ) / rayNormalizedDirection; + b3Vector3 t_max = ( select(aabb.m_max, aabb.m_min, isNegative) - rayOrigin ) / rayNormalizedDirection; + + b3Scalar t_min_final = 0.0f; + b3Scalar t_max_final = rayLength; + + //Must use fmin()/fmax(); if one of the parameters is NaN, then the parameter that is not NaN is returned. + //Behavior of min()/max() with NaNs is undefined. (See OpenCL Specification 1.2 [6.12.2] and [6.12.4]) + //Since the innermost fmin()/fmax() is always not NaN, this should never return NaN. + t_min_final = fmax( t_min.z, fmax(t_min.y, fmax(t_min.x, t_min_final)) ); + t_max_final = fmin( t_max.z, fmin(t_max.y, fmin(t_max.x, t_max_final)) ); + + return (t_min_final <= t_max_final); +} + +__kernel void plbvhRayTraverse(__global b3AabbCL* rigidAabbs, + + __global int* rootNodeIndex, + __global int2* internalNodeChildIndices, + __global b3AabbCL* internalNodeAabbs, + __global int2* internalNodeLeafIndexRanges, + __global SortDataCL* mortonCodesAndAabbIndices, + + __global b3RayInfo* rays, + + __global int* out_numRayRigidPairs, + __global int2* out_rayRigidPairs, + int maxRayRigidPairs, int numRays) +{ + int rayIndex = get_global_id(0); + if(rayIndex >= numRays) return; + + // + b3Vector3 rayFrom = rays[rayIndex].m_from; + b3Vector3 rayTo = rays[rayIndex].m_to; + b3Vector3 rayNormalizedDirection = b3Vector3_normalize(rayTo - rayFrom); + b3Scalar rayLength = b3Sqrt( b3Vector3_length2(rayTo - rayFrom) ); + + // + int stack[B3_PLVBH_TRAVERSE_MAX_STACK_SIZE]; + + int stackSize = 1; + stack[0] = *rootNodeIndex; + + while(stackSize) + { + int internalOrLeafNodeIndex = stack[ stackSize - 1 ]; + --stackSize; + + int isLeaf = isLeafNode(internalOrLeafNodeIndex); //Internal node if false + int bvhNodeIndex = getIndexWithInternalNodeMarkerRemoved(internalOrLeafNodeIndex); + + //bvhRigidIndex is not used if internal node + int bvhRigidIndex = (isLeaf) ? mortonCodesAndAabbIndices[bvhNodeIndex].m_value : -1; + + b3AabbCL bvhNodeAabb = (isLeaf) ? rigidAabbs[bvhRigidIndex] : internalNodeAabbs[bvhNodeIndex]; + if( rayIntersectsAabb(rayFrom, rayLength, rayNormalizedDirection, bvhNodeAabb) ) + { + if(isLeaf) + { + int2 rayRigidPair; + rayRigidPair.x = rayIndex; + rayRigidPair.y = rigidAabbs[bvhRigidIndex].m_minIndices[3]; + + int pairIndex = atomic_inc(out_numRayRigidPairs); + if(pairIndex < maxRayRigidPairs) out_rayRigidPairs[pairIndex] = rayRigidPair; + } + + if(!isLeaf) //Internal node + { + if(stackSize + 2 > B3_PLVBH_TRAVERSE_MAX_STACK_SIZE) + { + //Error + } + else + { + stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].x; + stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].y; + } + } + } + } +} + +__kernel void plbvhLargeAabbAabbTest(__global b3AabbCL* smallAabbs, __global b3AabbCL* largeAabbs, + __global int* out_numPairs, __global int4* out_overlappingPairs, + int maxPairs, int numLargeAabbRigids, int numSmallAabbRigids) +{ + int smallAabbIndex = get_global_id(0); + if(smallAabbIndex >= numSmallAabbRigids) return; + + b3AabbCL smallAabb = smallAabbs[smallAabbIndex]; + for(int i = 0; i < numLargeAabbRigids; ++i) + { + b3AabbCL largeAabb = largeAabbs[i]; + if( TestAabbAgainstAabb2(&smallAabb, &largeAabb) ) + { + int4 pair; + pair.x = largeAabb.m_minIndices[3]; + pair.y = smallAabb.m_minIndices[3]; + pair.z = NEW_PAIR_MARKER; + pair.w = NEW_PAIR_MARKER; + + int pairIndex = atomic_inc(out_numPairs); + if(pairIndex < maxPairs) out_overlappingPairs[pairIndex] = pair; + } + } +} +__kernel void plbvhLargeAabbRayTest(__global b3AabbCL* largeRigidAabbs, __global b3RayInfo* rays, + __global int* out_numRayRigidPairs, __global int2* out_rayRigidPairs, + int numLargeAabbRigids, int maxRayRigidPairs, int numRays) +{ + int rayIndex = get_global_id(0); + if(rayIndex >= numRays) return; + + b3Vector3 rayFrom = rays[rayIndex].m_from; + b3Vector3 rayTo = rays[rayIndex].m_to; + b3Vector3 rayNormalizedDirection = b3Vector3_normalize(rayTo - rayFrom); + b3Scalar rayLength = b3Sqrt( b3Vector3_length2(rayTo - rayFrom) ); + + for(int i = 0; i < numLargeAabbRigids; ++i) + { + b3AabbCL rigidAabb = largeRigidAabbs[i]; + if( rayIntersectsAabb(rayFrom, rayLength, rayNormalizedDirection, rigidAabb) ) + { + int2 rayRigidPair; + rayRigidPair.x = rayIndex; + rayRigidPair.y = rigidAabb.m_minIndices[3]; + + int pairIndex = atomic_inc(out_numRayRigidPairs); + if(pairIndex < maxRayRigidPairs) out_rayRigidPairs[pairIndex] = rayRigidPair; + } + } +} + + +//Set so that it is always greater than the actual common prefixes, and never selected as a parent node. +//If there are no duplicates, then the highest common prefix is 32 or 64, depending on the number of bits used for the z-curve. +//Duplicate common prefixes increase the highest common prefix at most by the number of bits used to index the leaf node. +//Since 32 bit ints are used to index leaf nodes, the max prefix is 64(32 + 32 bit z-curve) or 96(32 + 64 bit z-curve). +#define B3_PLBVH_INVALID_COMMON_PREFIX 128 + +#define B3_PLBVH_ROOT_NODE_MARKER -1 + +#define b3Int64 long + +int computeCommonPrefixLength(b3Int64 i, b3Int64 j) { return (int)clz(i ^ j); } +b3Int64 computeCommonPrefix(b3Int64 i, b3Int64 j) +{ + //This function only needs to return (i & j) in order for the algorithm to work, + //but it may help with debugging to mask out the lower bits. + + b3Int64 commonPrefixLength = (b3Int64)computeCommonPrefixLength(i, j); + + b3Int64 sharedBits = i & j; + b3Int64 bitmask = ((b3Int64)(~0)) << (64 - commonPrefixLength); //Set all bits after the common prefix to 0 + + return sharedBits & bitmask; +} + +//Same as computeCommonPrefixLength(), but allows for prefixes with different lengths +int getSharedPrefixLength(b3Int64 prefixA, int prefixLengthA, b3Int64 prefixB, int prefixLengthB) +{ + return b3Min( computeCommonPrefixLength(prefixA, prefixB), b3Min(prefixLengthA, prefixLengthB) ); +} + +__kernel void computeAdjacentPairCommonPrefix(__global SortDataCL* mortonCodesAndAabbIndices, + __global b3Int64* out_commonPrefixes, + __global int* out_commonPrefixLengths, + int numInternalNodes) +{ + int internalNodeIndex = get_global_id(0); + if (internalNodeIndex >= numInternalNodes) return; + + //Here, (internalNodeIndex + 1) is never out of bounds since it is a leaf node index, + //and the number of internal nodes is always numLeafNodes - 1 + int leftLeafIndex = internalNodeIndex; + int rightLeafIndex = internalNodeIndex + 1; + + int leftLeafMortonCode = mortonCodesAndAabbIndices[leftLeafIndex].m_key; + int rightLeafMortonCode = mortonCodesAndAabbIndices[rightLeafIndex].m_key; + + //Binary radix tree construction algorithm does not work if there are duplicate morton codes. + //Append the index of each leaf node to each morton code so that there are no duplicates. + //The algorithm also requires that the morton codes are sorted in ascending order; this requirement + //is also satisfied with this method, as (leftLeafIndex < rightLeafIndex) is always true. + // + //upsample(a, b) == ( ((b3Int64)a) << 32) | b + b3Int64 nonduplicateLeftMortonCode = upsample(leftLeafMortonCode, leftLeafIndex); + b3Int64 nonduplicateRightMortonCode = upsample(rightLeafMortonCode, rightLeafIndex); + + out_commonPrefixes[internalNodeIndex] = computeCommonPrefix(nonduplicateLeftMortonCode, nonduplicateRightMortonCode); + out_commonPrefixLengths[internalNodeIndex] = computeCommonPrefixLength(nonduplicateLeftMortonCode, nonduplicateRightMortonCode); +} + + +__kernel void buildBinaryRadixTreeLeafNodes(__global int* commonPrefixLengths, __global int* out_leafNodeParentNodes, + __global int2* out_childNodes, int numLeafNodes) +{ + int leafNodeIndex = get_global_id(0); + if (leafNodeIndex >= numLeafNodes) return; + + int numInternalNodes = numLeafNodes - 1; + + int leftSplitIndex = leafNodeIndex - 1; + int rightSplitIndex = leafNodeIndex; + + int leftCommonPrefix = (leftSplitIndex >= 0) ? commonPrefixLengths[leftSplitIndex] : B3_PLBVH_INVALID_COMMON_PREFIX; + int rightCommonPrefix = (rightSplitIndex < numInternalNodes) ? commonPrefixLengths[rightSplitIndex] : B3_PLBVH_INVALID_COMMON_PREFIX; + + //Parent node is the highest adjacent common prefix that is lower than the node's common prefix + //Leaf nodes are considered as having the highest common prefix + int isLeftHigherCommonPrefix = (leftCommonPrefix > rightCommonPrefix); + + //Handle cases for the edge nodes; the first and last node + //For leaf nodes, leftCommonPrefix and rightCommonPrefix should never both be B3_PLBVH_INVALID_COMMON_PREFIX + if(leftCommonPrefix == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherCommonPrefix = false; + if(rightCommonPrefix == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherCommonPrefix = true; + + int parentNodeIndex = (isLeftHigherCommonPrefix) ? leftSplitIndex : rightSplitIndex; + out_leafNodeParentNodes[leafNodeIndex] = parentNodeIndex; + + int isRightChild = (isLeftHigherCommonPrefix); //If the left node is the parent, then this node is its right child and vice versa + + //out_childNodesAsInt[0] == int2.x == left child + //out_childNodesAsInt[1] == int2.y == right child + int isLeaf = 1; + __global int* out_childNodesAsInt = (__global int*)(&out_childNodes[parentNodeIndex]); + out_childNodesAsInt[isRightChild] = getIndexWithInternalNodeMarkerSet(isLeaf, leafNodeIndex); +} + +__kernel void buildBinaryRadixTreeInternalNodes(__global b3Int64* commonPrefixes, __global int* commonPrefixLengths, + __global int2* out_childNodes, + __global int* out_internalNodeParentNodes, __global int* out_rootNodeIndex, + int numInternalNodes) +{ + int internalNodeIndex = get_group_id(0) * get_local_size(0) + get_local_id(0); + if(internalNodeIndex >= numInternalNodes) return; + + b3Int64 nodePrefix = commonPrefixes[internalNodeIndex]; + int nodePrefixLength = commonPrefixLengths[internalNodeIndex]; + +//#define USE_LINEAR_SEARCH +#ifdef USE_LINEAR_SEARCH + int leftIndex = -1; + int rightIndex = -1; + + //Find nearest element to left with a lower common prefix + for(int i = internalNodeIndex - 1; i >= 0; --i) + { + int nodeLeftSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, commonPrefixes[i], commonPrefixLengths[i]); + if(nodeLeftSharedPrefixLength < nodePrefixLength) + { + leftIndex = i; + break; + } + } + + //Find nearest element to right with a lower common prefix + for(int i = internalNodeIndex + 1; i < numInternalNodes; ++i) + { + int nodeRightSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, commonPrefixes[i], commonPrefixLengths[i]); + if(nodeRightSharedPrefixLength < nodePrefixLength) + { + rightIndex = i; + break; + } + } + +#else //Use binary search + + //Find nearest element to left with a lower common prefix + int leftIndex = -1; + { + int lower = 0; + int upper = internalNodeIndex - 1; + + while(lower <= upper) + { + int mid = (lower + upper) / 2; + b3Int64 midPrefix = commonPrefixes[mid]; + int midPrefixLength = commonPrefixLengths[mid]; + + int nodeMidSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, midPrefix, midPrefixLength); + if(nodeMidSharedPrefixLength < nodePrefixLength) + { + int right = mid + 1; + if(right < internalNodeIndex) + { + b3Int64 rightPrefix = commonPrefixes[right]; + int rightPrefixLength = commonPrefixLengths[right]; + + int nodeRightSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, rightPrefix, rightPrefixLength); + if(nodeRightSharedPrefixLength < nodePrefixLength) + { + lower = right; + leftIndex = right; + } + else + { + leftIndex = mid; + break; + } + } + else + { + leftIndex = mid; + break; + } + } + else upper = mid - 1; + } + } + + //Find nearest element to right with a lower common prefix + int rightIndex = -1; + { + int lower = internalNodeIndex + 1; + int upper = numInternalNodes - 1; + + while(lower <= upper) + { + int mid = (lower + upper) / 2; + b3Int64 midPrefix = commonPrefixes[mid]; + int midPrefixLength = commonPrefixLengths[mid]; + + int nodeMidSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, midPrefix, midPrefixLength); + if(nodeMidSharedPrefixLength < nodePrefixLength) + { + int left = mid - 1; + if(left > internalNodeIndex) + { + b3Int64 leftPrefix = commonPrefixes[left]; + int leftPrefixLength = commonPrefixLengths[left]; + + int nodeLeftSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, leftPrefix, leftPrefixLength); + if(nodeLeftSharedPrefixLength < nodePrefixLength) + { + upper = left; + rightIndex = left; + } + else + { + rightIndex = mid; + break; + } + } + else + { + rightIndex = mid; + break; + } + } + else lower = mid + 1; + } + } +#endif + + //Select parent + { + int leftPrefixLength = (leftIndex != -1) ? commonPrefixLengths[leftIndex] : B3_PLBVH_INVALID_COMMON_PREFIX; + int rightPrefixLength = (rightIndex != -1) ? commonPrefixLengths[rightIndex] : B3_PLBVH_INVALID_COMMON_PREFIX; + + int isLeftHigherPrefixLength = (leftPrefixLength > rightPrefixLength); + + if(leftPrefixLength == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherPrefixLength = false; + else if(rightPrefixLength == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherPrefixLength = true; + + int parentNodeIndex = (isLeftHigherPrefixLength) ? leftIndex : rightIndex; + + int isRootNode = (leftIndex == -1 && rightIndex == -1); + out_internalNodeParentNodes[internalNodeIndex] = (!isRootNode) ? parentNodeIndex : B3_PLBVH_ROOT_NODE_MARKER; + + int isLeaf = 0; + if(!isRootNode) + { + int isRightChild = (isLeftHigherPrefixLength); //If the left node is the parent, then this node is its right child and vice versa + + //out_childNodesAsInt[0] == int2.x == left child + //out_childNodesAsInt[1] == int2.y == right child + __global int* out_childNodesAsInt = (__global int*)(&out_childNodes[parentNodeIndex]); + out_childNodesAsInt[isRightChild] = getIndexWithInternalNodeMarkerSet(isLeaf, internalNodeIndex); + } + else *out_rootNodeIndex = getIndexWithInternalNodeMarkerSet(isLeaf, internalNodeIndex); + } +} + +__kernel void findDistanceFromRoot(__global int* rootNodeIndex, __global int* internalNodeParentNodes, + __global int* out_maxDistanceFromRoot, __global int* out_distanceFromRoot, int numInternalNodes) +{ + if( get_global_id(0) == 0 ) atomic_xchg(out_maxDistanceFromRoot, 0); + + int internalNodeIndex = get_global_id(0); + if(internalNodeIndex >= numInternalNodes) return; + + // + int distanceFromRoot = 0; + { + int parentIndex = internalNodeParentNodes[internalNodeIndex]; + while(parentIndex != B3_PLBVH_ROOT_NODE_MARKER) + { + parentIndex = internalNodeParentNodes[parentIndex]; + ++distanceFromRoot; + } + } + out_distanceFromRoot[internalNodeIndex] = distanceFromRoot; + + // + __local int localMaxDistanceFromRoot; + if( get_local_id(0) == 0 ) localMaxDistanceFromRoot = 0; + barrier(CLK_LOCAL_MEM_FENCE); + + atomic_max(&localMaxDistanceFromRoot, distanceFromRoot); + barrier(CLK_LOCAL_MEM_FENCE); + + if( get_local_id(0) == 0 ) atomic_max(out_maxDistanceFromRoot, localMaxDistanceFromRoot); +} + +__kernel void buildBinaryRadixTreeAabbsRecursive(__global int* distanceFromRoot, __global SortDataCL* mortonCodesAndAabbIndices, + __global int2* childNodes, + __global b3AabbCL* leafNodeAabbs, __global b3AabbCL* internalNodeAabbs, + int maxDistanceFromRoot, int processedDistance, int numInternalNodes) +{ + int internalNodeIndex = get_global_id(0); + if(internalNodeIndex >= numInternalNodes) return; + + int distance = distanceFromRoot[internalNodeIndex]; + + if(distance == processedDistance) + { + int leftChildIndex = childNodes[internalNodeIndex].x; + int rightChildIndex = childNodes[internalNodeIndex].y; + + int isLeftChildLeaf = isLeafNode(leftChildIndex); + int isRightChildLeaf = isLeafNode(rightChildIndex); + + leftChildIndex = getIndexWithInternalNodeMarkerRemoved(leftChildIndex); + rightChildIndex = getIndexWithInternalNodeMarkerRemoved(rightChildIndex); + + //leftRigidIndex/rightRigidIndex is not used if internal node + int leftRigidIndex = (isLeftChildLeaf) ? mortonCodesAndAabbIndices[leftChildIndex].m_value : -1; + int rightRigidIndex = (isRightChildLeaf) ? mortonCodesAndAabbIndices[rightChildIndex].m_value : -1; + + b3AabbCL leftChildAabb = (isLeftChildLeaf) ? leafNodeAabbs[leftRigidIndex] : internalNodeAabbs[leftChildIndex]; + b3AabbCL rightChildAabb = (isRightChildLeaf) ? leafNodeAabbs[rightRigidIndex] : internalNodeAabbs[rightChildIndex]; + + b3AabbCL mergedAabb; + mergedAabb.m_min = b3Min(leftChildAabb.m_min, rightChildAabb.m_min); + mergedAabb.m_max = b3Max(leftChildAabb.m_max, rightChildAabb.m_max); + internalNodeAabbs[internalNodeIndex] = mergedAabb; + } +} + +__kernel void findLeafIndexRanges(__global int2* internalNodeChildNodes, __global int2* out_leafIndexRanges, int numInternalNodes) +{ + int internalNodeIndex = get_global_id(0); + if(internalNodeIndex >= numInternalNodes) return; + + int numLeafNodes = numInternalNodes + 1; + + int2 childNodes = internalNodeChildNodes[internalNodeIndex]; + + int2 leafIndexRange; //x == min leaf index, y == max leaf index + + //Find lowest leaf index covered by this internal node + { + int lowestIndex = childNodes.x; //childNodes.x == Left child + while( !isLeafNode(lowestIndex) ) lowestIndex = internalNodeChildNodes[ getIndexWithInternalNodeMarkerRemoved(lowestIndex) ].x; + leafIndexRange.x = lowestIndex; + } + + //Find highest leaf index covered by this internal node + { + int highestIndex = childNodes.y; //childNodes.y == Right child + while( !isLeafNode(highestIndex) ) highestIndex = internalNodeChildNodes[ getIndexWithInternalNodeMarkerRemoved(highestIndex) ].y; + leafIndexRange.y = highestIndex; + } + + // + out_leafIndexRanges[internalNodeIndex] = leafIndexRange; +} diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvhKernels.h b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvhKernels.h new file mode 100644 index 000000000000..5eb8f45b164e --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvhKernels.h @@ -0,0 +1,729 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* parallelLinearBvhCL= \ +"/*\n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose,\n" +"including commercial applications, and to alter it and redistribute it freely,\n" +"subject to the following restrictions:\n" +"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.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"//Initial Author Jackson Lee, 2014\n" +"typedef float b3Scalar;\n" +"typedef float4 b3Vector3;\n" +"#define b3Max max\n" +"#define b3Min min\n" +"#define b3Sqrt sqrt\n" +"typedef struct\n" +"{\n" +" unsigned int m_key;\n" +" unsigned int m_value;\n" +"} SortDataCL;\n" +"typedef struct \n" +"{\n" +" union\n" +" {\n" +" float4 m_min;\n" +" float m_minElems[4];\n" +" int m_minIndices[4];\n" +" };\n" +" union\n" +" {\n" +" float4 m_max;\n" +" float m_maxElems[4];\n" +" int m_maxIndices[4];\n" +" };\n" +"} b3AabbCL;\n" +"unsigned int interleaveBits(unsigned int x)\n" +"{\n" +" //........ ........ ......12 3456789A //x\n" +" //....1..2 ..3..4.. 5..6..7. .8..9..A //x after interleaving bits\n" +" \n" +" //......12 3456789A ......12 3456789A //x ^ (x << 16)\n" +" //11111111 ........ ........ 11111111 //0x FF 00 00 FF\n" +" //......12 ........ ........ 3456789A //x = (x ^ (x << 16)) & 0xFF0000FF;\n" +" \n" +" //......12 ........ 3456789A 3456789A //x ^ (x << 8)\n" +" //......11 ........ 1111.... ....1111 //0x 03 00 F0 0F\n" +" //......12 ........ 3456.... ....789A //x = (x ^ (x << 8)) & 0x0300F00F;\n" +" \n" +" //..12..12 ....3456 3456.... 789A789A //x ^ (x << 4)\n" +" //......11 ....11.. ..11.... 11....11 //0x 03 0C 30 C3\n" +" //......12 ....34.. ..56.... 78....9A //x = (x ^ (x << 4)) & 0x030C30C3;\n" +" \n" +" //....1212 ..3434.. 5656..78 78..9A9A //x ^ (x << 2)\n" +" //....1..1 ..1..1.. 1..1..1. .1..1..1 //0x 09 24 92 49\n" +" //....1..2 ..3..4.. 5..6..7. .8..9..A //x = (x ^ (x << 2)) & 0x09249249;\n" +" \n" +" //........ ........ ......11 11111111 //0x000003FF\n" +" x &= 0x000003FF; //Clear all bits above bit 10\n" +" \n" +" x = (x ^ (x << 16)) & 0xFF0000FF;\n" +" x = (x ^ (x << 8)) & 0x0300F00F;\n" +" x = (x ^ (x << 4)) & 0x030C30C3;\n" +" x = (x ^ (x << 2)) & 0x09249249;\n" +" \n" +" return x;\n" +"}\n" +"unsigned int getMortonCode(unsigned int x, unsigned int y, unsigned int z)\n" +"{\n" +" return interleaveBits(x) << 0 | interleaveBits(y) << 1 | interleaveBits(z) << 2;\n" +"}\n" +"__kernel void separateAabbs(__global b3AabbCL* unseparatedAabbs, __global int* aabbIndices, __global b3AabbCL* out_aabbs, int numAabbsToSeparate)\n" +"{\n" +" int separatedAabbIndex = get_global_id(0);\n" +" if(separatedAabbIndex >= numAabbsToSeparate) return;\n" +" int unseparatedAabbIndex = aabbIndices[separatedAabbIndex];\n" +" out_aabbs[separatedAabbIndex] = unseparatedAabbs[unseparatedAabbIndex];\n" +"}\n" +"//Should replace with an optimized parallel reduction\n" +"__kernel void findAllNodesMergedAabb(__global b3AabbCL* out_mergedAabb, int numAabbsNeedingMerge)\n" +"{\n" +" //Each time this kernel is added to the command queue, \n" +" //the number of AABBs needing to be merged is halved\n" +" //\n" +" //Example with 159 AABBs:\n" +" // numRemainingAabbs == 159 / 2 + 159 % 2 == 80\n" +" // numMergedAabbs == 159 - 80 == 79\n" +" //So, indices [0, 78] are merged with [0 + 80, 78 + 80]\n" +" \n" +" int numRemainingAabbs = numAabbsNeedingMerge / 2 + numAabbsNeedingMerge % 2;\n" +" int numMergedAabbs = numAabbsNeedingMerge - numRemainingAabbs;\n" +" \n" +" int aabbIndex = get_global_id(0);\n" +" if(aabbIndex >= numMergedAabbs) return;\n" +" \n" +" int otherAabbIndex = aabbIndex + numRemainingAabbs;\n" +" \n" +" b3AabbCL aabb = out_mergedAabb[aabbIndex];\n" +" b3AabbCL otherAabb = out_mergedAabb[otherAabbIndex];\n" +" \n" +" b3AabbCL mergedAabb;\n" +" mergedAabb.m_min = b3Min(aabb.m_min, otherAabb.m_min);\n" +" mergedAabb.m_max = b3Max(aabb.m_max, otherAabb.m_max);\n" +" out_mergedAabb[aabbIndex] = mergedAabb;\n" +"}\n" +"__kernel void assignMortonCodesAndAabbIndicies(__global b3AabbCL* worldSpaceAabbs, __global b3AabbCL* mergedAabbOfAllNodes, \n" +" __global SortDataCL* out_mortonCodesAndAabbIndices, int numAabbs)\n" +"{\n" +" int leafNodeIndex = get_global_id(0); //Leaf node index == AABB index\n" +" if(leafNodeIndex >= numAabbs) return;\n" +" \n" +" b3AabbCL mergedAabb = mergedAabbOfAllNodes[0];\n" +" b3Vector3 gridCenter = (mergedAabb.m_min + mergedAabb.m_max) * 0.5f;\n" +" b3Vector3 gridCellSize = (mergedAabb.m_max - mergedAabb.m_min) / (float)1024;\n" +" \n" +" b3AabbCL aabb = worldSpaceAabbs[leafNodeIndex];\n" +" b3Vector3 aabbCenter = (aabb.m_min + aabb.m_max) * 0.5f;\n" +" b3Vector3 aabbCenterRelativeToGrid = aabbCenter - gridCenter;\n" +" \n" +" //Quantize into integer coordinates\n" +" //floor() is needed to prevent the center cell, at (0,0,0) from being twice the size\n" +" b3Vector3 gridPosition = aabbCenterRelativeToGrid / gridCellSize;\n" +" \n" +" int4 discretePosition;\n" +" discretePosition.x = (int)( (gridPosition.x >= 0.0f) ? gridPosition.x : floor(gridPosition.x) );\n" +" discretePosition.y = (int)( (gridPosition.y >= 0.0f) ? gridPosition.y : floor(gridPosition.y) );\n" +" discretePosition.z = (int)( (gridPosition.z >= 0.0f) ? gridPosition.z : floor(gridPosition.z) );\n" +" \n" +" //Clamp coordinates into [-512, 511], then convert range from [-512, 511] to [0, 1023]\n" +" discretePosition = b3Max( -512, b3Min(discretePosition, 511) );\n" +" discretePosition += 512;\n" +" \n" +" //Interleave bits(assign a morton code, also known as a z-curve)\n" +" unsigned int mortonCode = getMortonCode(discretePosition.x, discretePosition.y, discretePosition.z);\n" +" \n" +" //\n" +" SortDataCL mortonCodeIndexPair;\n" +" mortonCodeIndexPair.m_key = mortonCode;\n" +" mortonCodeIndexPair.m_value = leafNodeIndex;\n" +" \n" +" out_mortonCodesAndAabbIndices[leafNodeIndex] = mortonCodeIndexPair;\n" +"}\n" +"#define B3_PLVBH_TRAVERSE_MAX_STACK_SIZE 128\n" +"//The most significant bit(0x80000000) of a int32 is used to distinguish between leaf and internal nodes.\n" +"//If it is set, then the index is for an internal node; otherwise, it is a leaf node. \n" +"//In both cases, the bit should be cleared to access the actual node index.\n" +"int isLeafNode(int index) { return (index >> 31 == 0); }\n" +"int getIndexWithInternalNodeMarkerRemoved(int index) { return index & (~0x80000000); }\n" +"int getIndexWithInternalNodeMarkerSet(int isLeaf, int index) { return (isLeaf) ? index : (index | 0x80000000); }\n" +"//From sap.cl\n" +"#define NEW_PAIR_MARKER -1\n" +"bool TestAabbAgainstAabb2(const b3AabbCL* aabb1, const b3AabbCL* aabb2)\n" +"{\n" +" bool overlap = true;\n" +" overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n" +" overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n" +" overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n" +" return overlap;\n" +"}\n" +"//From sap.cl\n" +"__kernel void plbvhCalculateOverlappingPairs(__global b3AabbCL* rigidAabbs, \n" +" __global int* rootNodeIndex, \n" +" __global int2* internalNodeChildIndices, \n" +" __global b3AabbCL* internalNodeAabbs,\n" +" __global int2* internalNodeLeafIndexRanges,\n" +" \n" +" __global SortDataCL* mortonCodesAndAabbIndices,\n" +" __global int* out_numPairs, __global int4* out_overlappingPairs, \n" +" int maxPairs, int numQueryAabbs)\n" +"{\n" +" //Using get_group_id()/get_local_id() is Faster than get_global_id(0) since\n" +" //mortonCodesAndAabbIndices[] contains rigid body indices sorted along the z-curve (more spatially coherent)\n" +" int queryBvhNodeIndex = get_group_id(0) * get_local_size(0) + get_local_id(0);\n" +" if(queryBvhNodeIndex >= numQueryAabbs) return;\n" +" \n" +" int queryRigidIndex = mortonCodesAndAabbIndices[queryBvhNodeIndex].m_value;\n" +" b3AabbCL queryAabb = rigidAabbs[queryRigidIndex];\n" +" \n" +" int stack[B3_PLVBH_TRAVERSE_MAX_STACK_SIZE];\n" +" \n" +" int stackSize = 1;\n" +" stack[0] = *rootNodeIndex;\n" +" \n" +" while(stackSize)\n" +" {\n" +" int internalOrLeafNodeIndex = stack[ stackSize - 1 ];\n" +" --stackSize;\n" +" \n" +" int isLeaf = isLeafNode(internalOrLeafNodeIndex); //Internal node if false\n" +" int bvhNodeIndex = getIndexWithInternalNodeMarkerRemoved(internalOrLeafNodeIndex);\n" +" \n" +" //Optimization - if the BVH is structured as a binary radix tree, then\n" +" //each internal node corresponds to a contiguous range of leaf nodes(internalNodeLeafIndexRanges[]).\n" +" //This can be used to avoid testing each AABB-AABB pair twice, including preventing each node from colliding with itself.\n" +" {\n" +" int highestLeafIndex = (isLeaf) ? bvhNodeIndex : internalNodeLeafIndexRanges[bvhNodeIndex].y;\n" +" if(highestLeafIndex <= queryBvhNodeIndex) continue;\n" +" }\n" +" \n" +" //bvhRigidIndex is not used if internal node\n" +" int bvhRigidIndex = (isLeaf) ? mortonCodesAndAabbIndices[bvhNodeIndex].m_value : -1;\n" +" \n" +" b3AabbCL bvhNodeAabb = (isLeaf) ? rigidAabbs[bvhRigidIndex] : internalNodeAabbs[bvhNodeIndex];\n" +" if( TestAabbAgainstAabb2(&queryAabb, &bvhNodeAabb) )\n" +" {\n" +" if(isLeaf)\n" +" {\n" +" int4 pair;\n" +" pair.x = rigidAabbs[queryRigidIndex].m_minIndices[3];\n" +" pair.y = rigidAabbs[bvhRigidIndex].m_minIndices[3];\n" +" pair.z = NEW_PAIR_MARKER;\n" +" pair.w = NEW_PAIR_MARKER;\n" +" \n" +" int pairIndex = atomic_inc(out_numPairs);\n" +" if(pairIndex < maxPairs) out_overlappingPairs[pairIndex] = pair;\n" +" }\n" +" \n" +" if(!isLeaf) //Internal node\n" +" {\n" +" if(stackSize + 2 > B3_PLVBH_TRAVERSE_MAX_STACK_SIZE)\n" +" {\n" +" //Error\n" +" }\n" +" else\n" +" {\n" +" stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].x;\n" +" stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].y;\n" +" }\n" +" }\n" +" }\n" +" \n" +" }\n" +"}\n" +"//From rayCastKernels.cl\n" +"typedef struct\n" +"{\n" +" float4 m_from;\n" +" float4 m_to;\n" +"} b3RayInfo;\n" +"//From rayCastKernels.cl\n" +"b3Vector3 b3Vector3_normalize(b3Vector3 v)\n" +"{\n" +" b3Vector3 normal = (b3Vector3){v.x, v.y, v.z, 0.f};\n" +" return normalize(normal); //OpenCL normalize == vector4 normalize\n" +"}\n" +"b3Scalar b3Vector3_length2(b3Vector3 v) { return v.x*v.x + v.y*v.y + v.z*v.z; }\n" +"b3Scalar b3Vector3_dot(b3Vector3 a, b3Vector3 b) { return a.x*b.x + a.y*b.y + a.z*b.z; }\n" +"int rayIntersectsAabb(b3Vector3 rayOrigin, b3Scalar rayLength, b3Vector3 rayNormalizedDirection, b3AabbCL aabb)\n" +"{\n" +" //AABB is considered as 3 pairs of 2 planes( {x_min, x_max}, {y_min, y_max}, {z_min, z_max} ).\n" +" //t_min is the point of intersection with the closer plane, t_max is the point of intersection with the farther plane.\n" +" //\n" +" //if (rayNormalizedDirection.x < 0.0f), then max.x will be the near plane \n" +" //and min.x will be the far plane; otherwise, it is reversed.\n" +" //\n" +" //In order for there to be a collision, the t_min and t_max of each pair must overlap.\n" +" //This can be tested for by selecting the highest t_min and lowest t_max and comparing them.\n" +" \n" +" int4 isNegative = isless( rayNormalizedDirection, ((b3Vector3){0.0f, 0.0f, 0.0f, 0.0f}) ); //isless(x,y) returns (x < y)\n" +" \n" +" //When using vector types, the select() function checks the most signficant bit, \n" +" //but isless() sets the least significant bit.\n" +" isNegative <<= 31;\n" +" //select(b, a, condition) == condition ? a : b\n" +" //When using select() with vector types, (condition[i]) is true if its most significant bit is 1\n" +" b3Vector3 t_min = ( select(aabb.m_min, aabb.m_max, isNegative) - rayOrigin ) / rayNormalizedDirection;\n" +" b3Vector3 t_max = ( select(aabb.m_max, aabb.m_min, isNegative) - rayOrigin ) / rayNormalizedDirection;\n" +" \n" +" b3Scalar t_min_final = 0.0f;\n" +" b3Scalar t_max_final = rayLength;\n" +" \n" +" //Must use fmin()/fmax(); if one of the parameters is NaN, then the parameter that is not NaN is returned. \n" +" //Behavior of min()/max() with NaNs is undefined. (See OpenCL Specification 1.2 [6.12.2] and [6.12.4])\n" +" //Since the innermost fmin()/fmax() is always not NaN, this should never return NaN.\n" +" t_min_final = fmax( t_min.z, fmax(t_min.y, fmax(t_min.x, t_min_final)) );\n" +" t_max_final = fmin( t_max.z, fmin(t_max.y, fmin(t_max.x, t_max_final)) );\n" +" \n" +" return (t_min_final <= t_max_final);\n" +"}\n" +"__kernel void plbvhRayTraverse(__global b3AabbCL* rigidAabbs,\n" +" __global int* rootNodeIndex, \n" +" __global int2* internalNodeChildIndices, \n" +" __global b3AabbCL* internalNodeAabbs,\n" +" __global int2* internalNodeLeafIndexRanges,\n" +" __global SortDataCL* mortonCodesAndAabbIndices,\n" +" \n" +" __global b3RayInfo* rays,\n" +" \n" +" __global int* out_numRayRigidPairs, \n" +" __global int2* out_rayRigidPairs,\n" +" int maxRayRigidPairs, int numRays)\n" +"{\n" +" int rayIndex = get_global_id(0);\n" +" if(rayIndex >= numRays) return;\n" +" \n" +" //\n" +" b3Vector3 rayFrom = rays[rayIndex].m_from;\n" +" b3Vector3 rayTo = rays[rayIndex].m_to;\n" +" b3Vector3 rayNormalizedDirection = b3Vector3_normalize(rayTo - rayFrom);\n" +" b3Scalar rayLength = b3Sqrt( b3Vector3_length2(rayTo - rayFrom) );\n" +" \n" +" //\n" +" int stack[B3_PLVBH_TRAVERSE_MAX_STACK_SIZE];\n" +" \n" +" int stackSize = 1;\n" +" stack[0] = *rootNodeIndex;\n" +" \n" +" while(stackSize)\n" +" {\n" +" int internalOrLeafNodeIndex = stack[ stackSize - 1 ];\n" +" --stackSize;\n" +" \n" +" int isLeaf = isLeafNode(internalOrLeafNodeIndex); //Internal node if false\n" +" int bvhNodeIndex = getIndexWithInternalNodeMarkerRemoved(internalOrLeafNodeIndex);\n" +" \n" +" //bvhRigidIndex is not used if internal node\n" +" int bvhRigidIndex = (isLeaf) ? mortonCodesAndAabbIndices[bvhNodeIndex].m_value : -1;\n" +" \n" +" b3AabbCL bvhNodeAabb = (isLeaf) ? rigidAabbs[bvhRigidIndex] : internalNodeAabbs[bvhNodeIndex];\n" +" if( rayIntersectsAabb(rayFrom, rayLength, rayNormalizedDirection, bvhNodeAabb) )\n" +" {\n" +" if(isLeaf)\n" +" {\n" +" int2 rayRigidPair;\n" +" rayRigidPair.x = rayIndex;\n" +" rayRigidPair.y = rigidAabbs[bvhRigidIndex].m_minIndices[3];\n" +" \n" +" int pairIndex = atomic_inc(out_numRayRigidPairs);\n" +" if(pairIndex < maxRayRigidPairs) out_rayRigidPairs[pairIndex] = rayRigidPair;\n" +" }\n" +" \n" +" if(!isLeaf) //Internal node\n" +" {\n" +" if(stackSize + 2 > B3_PLVBH_TRAVERSE_MAX_STACK_SIZE)\n" +" {\n" +" //Error\n" +" }\n" +" else\n" +" {\n" +" stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].x;\n" +" stack[ stackSize++ ] = internalNodeChildIndices[bvhNodeIndex].y;\n" +" }\n" +" }\n" +" }\n" +" }\n" +"}\n" +"__kernel void plbvhLargeAabbAabbTest(__global b3AabbCL* smallAabbs, __global b3AabbCL* largeAabbs, \n" +" __global int* out_numPairs, __global int4* out_overlappingPairs, \n" +" int maxPairs, int numLargeAabbRigids, int numSmallAabbRigids)\n" +"{\n" +" int smallAabbIndex = get_global_id(0);\n" +" if(smallAabbIndex >= numSmallAabbRigids) return;\n" +" \n" +" b3AabbCL smallAabb = smallAabbs[smallAabbIndex];\n" +" for(int i = 0; i < numLargeAabbRigids; ++i)\n" +" {\n" +" b3AabbCL largeAabb = largeAabbs[i];\n" +" if( TestAabbAgainstAabb2(&smallAabb, &largeAabb) )\n" +" {\n" +" int4 pair;\n" +" pair.x = largeAabb.m_minIndices[3];\n" +" pair.y = smallAabb.m_minIndices[3];\n" +" pair.z = NEW_PAIR_MARKER;\n" +" pair.w = NEW_PAIR_MARKER;\n" +" \n" +" int pairIndex = atomic_inc(out_numPairs);\n" +" if(pairIndex < maxPairs) out_overlappingPairs[pairIndex] = pair;\n" +" }\n" +" }\n" +"}\n" +"__kernel void plbvhLargeAabbRayTest(__global b3AabbCL* largeRigidAabbs, __global b3RayInfo* rays,\n" +" __global int* out_numRayRigidPairs, __global int2* out_rayRigidPairs,\n" +" int numLargeAabbRigids, int maxRayRigidPairs, int numRays)\n" +"{\n" +" int rayIndex = get_global_id(0);\n" +" if(rayIndex >= numRays) return;\n" +" \n" +" b3Vector3 rayFrom = rays[rayIndex].m_from;\n" +" b3Vector3 rayTo = rays[rayIndex].m_to;\n" +" b3Vector3 rayNormalizedDirection = b3Vector3_normalize(rayTo - rayFrom);\n" +" b3Scalar rayLength = b3Sqrt( b3Vector3_length2(rayTo - rayFrom) );\n" +" \n" +" for(int i = 0; i < numLargeAabbRigids; ++i)\n" +" {\n" +" b3AabbCL rigidAabb = largeRigidAabbs[i];\n" +" if( rayIntersectsAabb(rayFrom, rayLength, rayNormalizedDirection, rigidAabb) )\n" +" {\n" +" int2 rayRigidPair;\n" +" rayRigidPair.x = rayIndex;\n" +" rayRigidPair.y = rigidAabb.m_minIndices[3];\n" +" \n" +" int pairIndex = atomic_inc(out_numRayRigidPairs);\n" +" if(pairIndex < maxRayRigidPairs) out_rayRigidPairs[pairIndex] = rayRigidPair;\n" +" }\n" +" }\n" +"}\n" +"//Set so that it is always greater than the actual common prefixes, and never selected as a parent node.\n" +"//If there are no duplicates, then the highest common prefix is 32 or 64, depending on the number of bits used for the z-curve.\n" +"//Duplicate common prefixes increase the highest common prefix at most by the number of bits used to index the leaf node.\n" +"//Since 32 bit ints are used to index leaf nodes, the max prefix is 64(32 + 32 bit z-curve) or 96(32 + 64 bit z-curve).\n" +"#define B3_PLBVH_INVALID_COMMON_PREFIX 128\n" +"#define B3_PLBVH_ROOT_NODE_MARKER -1\n" +"#define b3Int64 long\n" +"int computeCommonPrefixLength(b3Int64 i, b3Int64 j) { return (int)clz(i ^ j); }\n" +"b3Int64 computeCommonPrefix(b3Int64 i, b3Int64 j) \n" +"{\n" +" //This function only needs to return (i & j) in order for the algorithm to work,\n" +" //but it may help with debugging to mask out the lower bits.\n" +" b3Int64 commonPrefixLength = (b3Int64)computeCommonPrefixLength(i, j);\n" +" b3Int64 sharedBits = i & j;\n" +" b3Int64 bitmask = ((b3Int64)(~0)) << (64 - commonPrefixLength); //Set all bits after the common prefix to 0\n" +" \n" +" return sharedBits & bitmask;\n" +"}\n" +"//Same as computeCommonPrefixLength(), but allows for prefixes with different lengths\n" +"int getSharedPrefixLength(b3Int64 prefixA, int prefixLengthA, b3Int64 prefixB, int prefixLengthB)\n" +"{\n" +" return b3Min( computeCommonPrefixLength(prefixA, prefixB), b3Min(prefixLengthA, prefixLengthB) );\n" +"}\n" +"__kernel void computeAdjacentPairCommonPrefix(__global SortDataCL* mortonCodesAndAabbIndices,\n" +" __global b3Int64* out_commonPrefixes,\n" +" __global int* out_commonPrefixLengths,\n" +" int numInternalNodes)\n" +"{\n" +" int internalNodeIndex = get_global_id(0);\n" +" if (internalNodeIndex >= numInternalNodes) return;\n" +" \n" +" //Here, (internalNodeIndex + 1) is never out of bounds since it is a leaf node index,\n" +" //and the number of internal nodes is always numLeafNodes - 1\n" +" int leftLeafIndex = internalNodeIndex;\n" +" int rightLeafIndex = internalNodeIndex + 1;\n" +" \n" +" int leftLeafMortonCode = mortonCodesAndAabbIndices[leftLeafIndex].m_key;\n" +" int rightLeafMortonCode = mortonCodesAndAabbIndices[rightLeafIndex].m_key;\n" +" \n" +" //Binary radix tree construction algorithm does not work if there are duplicate morton codes.\n" +" //Append the index of each leaf node to each morton code so that there are no duplicates.\n" +" //The algorithm also requires that the morton codes are sorted in ascending order; this requirement\n" +" //is also satisfied with this method, as (leftLeafIndex < rightLeafIndex) is always true.\n" +" //\n" +" //upsample(a, b) == ( ((b3Int64)a) << 32) | b\n" +" b3Int64 nonduplicateLeftMortonCode = upsample(leftLeafMortonCode, leftLeafIndex);\n" +" b3Int64 nonduplicateRightMortonCode = upsample(rightLeafMortonCode, rightLeafIndex);\n" +" \n" +" out_commonPrefixes[internalNodeIndex] = computeCommonPrefix(nonduplicateLeftMortonCode, nonduplicateRightMortonCode);\n" +" out_commonPrefixLengths[internalNodeIndex] = computeCommonPrefixLength(nonduplicateLeftMortonCode, nonduplicateRightMortonCode);\n" +"}\n" +"__kernel void buildBinaryRadixTreeLeafNodes(__global int* commonPrefixLengths, __global int* out_leafNodeParentNodes,\n" +" __global int2* out_childNodes, int numLeafNodes)\n" +"{\n" +" int leafNodeIndex = get_global_id(0);\n" +" if (leafNodeIndex >= numLeafNodes) return;\n" +" \n" +" int numInternalNodes = numLeafNodes - 1;\n" +" \n" +" int leftSplitIndex = leafNodeIndex - 1;\n" +" int rightSplitIndex = leafNodeIndex;\n" +" \n" +" int leftCommonPrefix = (leftSplitIndex >= 0) ? commonPrefixLengths[leftSplitIndex] : B3_PLBVH_INVALID_COMMON_PREFIX;\n" +" int rightCommonPrefix = (rightSplitIndex < numInternalNodes) ? commonPrefixLengths[rightSplitIndex] : B3_PLBVH_INVALID_COMMON_PREFIX;\n" +" \n" +" //Parent node is the highest adjacent common prefix that is lower than the node's common prefix\n" +" //Leaf nodes are considered as having the highest common prefix\n" +" int isLeftHigherCommonPrefix = (leftCommonPrefix > rightCommonPrefix);\n" +" \n" +" //Handle cases for the edge nodes; the first and last node\n" +" //For leaf nodes, leftCommonPrefix and rightCommonPrefix should never both be B3_PLBVH_INVALID_COMMON_PREFIX\n" +" if(leftCommonPrefix == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherCommonPrefix = false;\n" +" if(rightCommonPrefix == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherCommonPrefix = true;\n" +" \n" +" int parentNodeIndex = (isLeftHigherCommonPrefix) ? leftSplitIndex : rightSplitIndex;\n" +" out_leafNodeParentNodes[leafNodeIndex] = parentNodeIndex;\n" +" \n" +" int isRightChild = (isLeftHigherCommonPrefix); //If the left node is the parent, then this node is its right child and vice versa\n" +" \n" +" //out_childNodesAsInt[0] == int2.x == left child\n" +" //out_childNodesAsInt[1] == int2.y == right child\n" +" int isLeaf = 1;\n" +" __global int* out_childNodesAsInt = (__global int*)(&out_childNodes[parentNodeIndex]);\n" +" out_childNodesAsInt[isRightChild] = getIndexWithInternalNodeMarkerSet(isLeaf, leafNodeIndex);\n" +"}\n" +"__kernel void buildBinaryRadixTreeInternalNodes(__global b3Int64* commonPrefixes, __global int* commonPrefixLengths,\n" +" __global int2* out_childNodes,\n" +" __global int* out_internalNodeParentNodes, __global int* out_rootNodeIndex,\n" +" int numInternalNodes)\n" +"{\n" +" int internalNodeIndex = get_group_id(0) * get_local_size(0) + get_local_id(0);\n" +" if(internalNodeIndex >= numInternalNodes) return;\n" +" \n" +" b3Int64 nodePrefix = commonPrefixes[internalNodeIndex];\n" +" int nodePrefixLength = commonPrefixLengths[internalNodeIndex];\n" +" \n" +"//#define USE_LINEAR_SEARCH\n" +"#ifdef USE_LINEAR_SEARCH\n" +" int leftIndex = -1;\n" +" int rightIndex = -1;\n" +" \n" +" //Find nearest element to left with a lower common prefix\n" +" for(int i = internalNodeIndex - 1; i >= 0; --i)\n" +" {\n" +" int nodeLeftSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, commonPrefixes[i], commonPrefixLengths[i]);\n" +" if(nodeLeftSharedPrefixLength < nodePrefixLength)\n" +" {\n" +" leftIndex = i;\n" +" break;\n" +" }\n" +" }\n" +" \n" +" //Find nearest element to right with a lower common prefix\n" +" for(int i = internalNodeIndex + 1; i < numInternalNodes; ++i)\n" +" {\n" +" int nodeRightSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, commonPrefixes[i], commonPrefixLengths[i]);\n" +" if(nodeRightSharedPrefixLength < nodePrefixLength)\n" +" {\n" +" rightIndex = i;\n" +" break;\n" +" }\n" +" }\n" +" \n" +"#else //Use binary search\n" +" //Find nearest element to left with a lower common prefix\n" +" int leftIndex = -1;\n" +" {\n" +" int lower = 0;\n" +" int upper = internalNodeIndex - 1;\n" +" \n" +" while(lower <= upper)\n" +" {\n" +" int mid = (lower + upper) / 2;\n" +" b3Int64 midPrefix = commonPrefixes[mid];\n" +" int midPrefixLength = commonPrefixLengths[mid];\n" +" \n" +" int nodeMidSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, midPrefix, midPrefixLength);\n" +" if(nodeMidSharedPrefixLength < nodePrefixLength) \n" +" {\n" +" int right = mid + 1;\n" +" if(right < internalNodeIndex)\n" +" {\n" +" b3Int64 rightPrefix = commonPrefixes[right];\n" +" int rightPrefixLength = commonPrefixLengths[right];\n" +" \n" +" int nodeRightSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, rightPrefix, rightPrefixLength);\n" +" if(nodeRightSharedPrefixLength < nodePrefixLength) \n" +" {\n" +" lower = right;\n" +" leftIndex = right;\n" +" }\n" +" else \n" +" {\n" +" leftIndex = mid;\n" +" break;\n" +" }\n" +" }\n" +" else \n" +" {\n" +" leftIndex = mid;\n" +" break;\n" +" }\n" +" }\n" +" else upper = mid - 1;\n" +" }\n" +" }\n" +" \n" +" //Find nearest element to right with a lower common prefix\n" +" int rightIndex = -1;\n" +" {\n" +" int lower = internalNodeIndex + 1;\n" +" int upper = numInternalNodes - 1;\n" +" \n" +" while(lower <= upper)\n" +" {\n" +" int mid = (lower + upper) / 2;\n" +" b3Int64 midPrefix = commonPrefixes[mid];\n" +" int midPrefixLength = commonPrefixLengths[mid];\n" +" \n" +" int nodeMidSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, midPrefix, midPrefixLength);\n" +" if(nodeMidSharedPrefixLength < nodePrefixLength) \n" +" {\n" +" int left = mid - 1;\n" +" if(left > internalNodeIndex)\n" +" {\n" +" b3Int64 leftPrefix = commonPrefixes[left];\n" +" int leftPrefixLength = commonPrefixLengths[left];\n" +" \n" +" int nodeLeftSharedPrefixLength = getSharedPrefixLength(nodePrefix, nodePrefixLength, leftPrefix, leftPrefixLength);\n" +" if(nodeLeftSharedPrefixLength < nodePrefixLength) \n" +" {\n" +" upper = left;\n" +" rightIndex = left;\n" +" }\n" +" else \n" +" {\n" +" rightIndex = mid;\n" +" break;\n" +" }\n" +" }\n" +" else \n" +" {\n" +" rightIndex = mid;\n" +" break;\n" +" }\n" +" }\n" +" else lower = mid + 1;\n" +" }\n" +" }\n" +"#endif\n" +" \n" +" //Select parent\n" +" {\n" +" int leftPrefixLength = (leftIndex != -1) ? commonPrefixLengths[leftIndex] : B3_PLBVH_INVALID_COMMON_PREFIX;\n" +" int rightPrefixLength = (rightIndex != -1) ? commonPrefixLengths[rightIndex] : B3_PLBVH_INVALID_COMMON_PREFIX;\n" +" \n" +" int isLeftHigherPrefixLength = (leftPrefixLength > rightPrefixLength);\n" +" \n" +" if(leftPrefixLength == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherPrefixLength = false;\n" +" else if(rightPrefixLength == B3_PLBVH_INVALID_COMMON_PREFIX) isLeftHigherPrefixLength = true;\n" +" \n" +" int parentNodeIndex = (isLeftHigherPrefixLength) ? leftIndex : rightIndex;\n" +" \n" +" int isRootNode = (leftIndex == -1 && rightIndex == -1);\n" +" out_internalNodeParentNodes[internalNodeIndex] = (!isRootNode) ? parentNodeIndex : B3_PLBVH_ROOT_NODE_MARKER;\n" +" \n" +" int isLeaf = 0;\n" +" if(!isRootNode)\n" +" {\n" +" int isRightChild = (isLeftHigherPrefixLength); //If the left node is the parent, then this node is its right child and vice versa\n" +" \n" +" //out_childNodesAsInt[0] == int2.x == left child\n" +" //out_childNodesAsInt[1] == int2.y == right child\n" +" __global int* out_childNodesAsInt = (__global int*)(&out_childNodes[parentNodeIndex]);\n" +" out_childNodesAsInt[isRightChild] = getIndexWithInternalNodeMarkerSet(isLeaf, internalNodeIndex);\n" +" }\n" +" else *out_rootNodeIndex = getIndexWithInternalNodeMarkerSet(isLeaf, internalNodeIndex);\n" +" }\n" +"}\n" +"__kernel void findDistanceFromRoot(__global int* rootNodeIndex, __global int* internalNodeParentNodes,\n" +" __global int* out_maxDistanceFromRoot, __global int* out_distanceFromRoot, int numInternalNodes)\n" +"{\n" +" if( get_global_id(0) == 0 ) atomic_xchg(out_maxDistanceFromRoot, 0);\n" +" int internalNodeIndex = get_global_id(0);\n" +" if(internalNodeIndex >= numInternalNodes) return;\n" +" \n" +" //\n" +" int distanceFromRoot = 0;\n" +" {\n" +" int parentIndex = internalNodeParentNodes[internalNodeIndex];\n" +" while(parentIndex != B3_PLBVH_ROOT_NODE_MARKER)\n" +" {\n" +" parentIndex = internalNodeParentNodes[parentIndex];\n" +" ++distanceFromRoot;\n" +" }\n" +" }\n" +" out_distanceFromRoot[internalNodeIndex] = distanceFromRoot;\n" +" \n" +" //\n" +" __local int localMaxDistanceFromRoot;\n" +" if( get_local_id(0) == 0 ) localMaxDistanceFromRoot = 0;\n" +" barrier(CLK_LOCAL_MEM_FENCE);\n" +" \n" +" atomic_max(&localMaxDistanceFromRoot, distanceFromRoot);\n" +" barrier(CLK_LOCAL_MEM_FENCE);\n" +" \n" +" if( get_local_id(0) == 0 ) atomic_max(out_maxDistanceFromRoot, localMaxDistanceFromRoot);\n" +"}\n" +"__kernel void buildBinaryRadixTreeAabbsRecursive(__global int* distanceFromRoot, __global SortDataCL* mortonCodesAndAabbIndices,\n" +" __global int2* childNodes,\n" +" __global b3AabbCL* leafNodeAabbs, __global b3AabbCL* internalNodeAabbs,\n" +" int maxDistanceFromRoot, int processedDistance, int numInternalNodes)\n" +"{\n" +" int internalNodeIndex = get_global_id(0);\n" +" if(internalNodeIndex >= numInternalNodes) return;\n" +" \n" +" int distance = distanceFromRoot[internalNodeIndex];\n" +" \n" +" if(distance == processedDistance)\n" +" {\n" +" int leftChildIndex = childNodes[internalNodeIndex].x;\n" +" int rightChildIndex = childNodes[internalNodeIndex].y;\n" +" \n" +" int isLeftChildLeaf = isLeafNode(leftChildIndex);\n" +" int isRightChildLeaf = isLeafNode(rightChildIndex);\n" +" \n" +" leftChildIndex = getIndexWithInternalNodeMarkerRemoved(leftChildIndex);\n" +" rightChildIndex = getIndexWithInternalNodeMarkerRemoved(rightChildIndex);\n" +" \n" +" //leftRigidIndex/rightRigidIndex is not used if internal node\n" +" int leftRigidIndex = (isLeftChildLeaf) ? mortonCodesAndAabbIndices[leftChildIndex].m_value : -1;\n" +" int rightRigidIndex = (isRightChildLeaf) ? mortonCodesAndAabbIndices[rightChildIndex].m_value : -1;\n" +" \n" +" b3AabbCL leftChildAabb = (isLeftChildLeaf) ? leafNodeAabbs[leftRigidIndex] : internalNodeAabbs[leftChildIndex];\n" +" b3AabbCL rightChildAabb = (isRightChildLeaf) ? leafNodeAabbs[rightRigidIndex] : internalNodeAabbs[rightChildIndex];\n" +" \n" +" b3AabbCL mergedAabb;\n" +" mergedAabb.m_min = b3Min(leftChildAabb.m_min, rightChildAabb.m_min);\n" +" mergedAabb.m_max = b3Max(leftChildAabb.m_max, rightChildAabb.m_max);\n" +" internalNodeAabbs[internalNodeIndex] = mergedAabb;\n" +" }\n" +"}\n" +"__kernel void findLeafIndexRanges(__global int2* internalNodeChildNodes, __global int2* out_leafIndexRanges, int numInternalNodes)\n" +"{\n" +" int internalNodeIndex = get_global_id(0);\n" +" if(internalNodeIndex >= numInternalNodes) return;\n" +" \n" +" int numLeafNodes = numInternalNodes + 1;\n" +" \n" +" int2 childNodes = internalNodeChildNodes[internalNodeIndex];\n" +" \n" +" int2 leafIndexRange; //x == min leaf index, y == max leaf index\n" +" \n" +" //Find lowest leaf index covered by this internal node\n" +" {\n" +" int lowestIndex = childNodes.x; //childNodes.x == Left child\n" +" while( !isLeafNode(lowestIndex) ) lowestIndex = internalNodeChildNodes[ getIndexWithInternalNodeMarkerRemoved(lowestIndex) ].x;\n" +" leafIndexRange.x = lowestIndex;\n" +" }\n" +" \n" +" //Find highest leaf index covered by this internal node\n" +" {\n" +" int highestIndex = childNodes.y; //childNodes.y == Right child\n" +" while( !isLeafNode(highestIndex) ) highestIndex = internalNodeChildNodes[ getIndexWithInternalNodeMarkerRemoved(highestIndex) ].y;\n" +" leafIndexRange.y = highestIndex;\n" +" }\n" +" \n" +" //\n" +" out_leafIndexRanges[internalNodeIndex] = leafIndexRange;\n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl new file mode 100644 index 000000000000..93f77a643355 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl @@ -0,0 +1,389 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + +#define NEW_PAIR_MARKER -1 + +typedef struct +{ + union + { + float4 m_min; + float m_minElems[4]; + int m_minIndices[4]; + }; + union + { + float4 m_max; + float m_maxElems[4]; + int m_maxIndices[4]; + }; +} btAabbCL; + + +/// conservative test for overlap between two aabbs +bool TestAabbAgainstAabb2(const btAabbCL* aabb1, __local const btAabbCL* aabb2); +bool TestAabbAgainstAabb2(const btAabbCL* aabb1, __local const btAabbCL* aabb2) +{ + bool overlap = true; + overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap; + overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap; + overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap; + return overlap; +} +bool TestAabbAgainstAabb2GlobalGlobal(__global const btAabbCL* aabb1, __global const btAabbCL* aabb2); +bool TestAabbAgainstAabb2GlobalGlobal(__global const btAabbCL* aabb1, __global const btAabbCL* aabb2) +{ + bool overlap = true; + overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap; + overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap; + overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap; + return overlap; +} + +bool TestAabbAgainstAabb2Global(const btAabbCL* aabb1, __global const btAabbCL* aabb2); +bool TestAabbAgainstAabb2Global(const btAabbCL* aabb1, __global const btAabbCL* aabb2) +{ + bool overlap = true; + overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap; + overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap; + overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap; + return overlap; +} + + +__kernel void computePairsKernelTwoArrays( __global const btAabbCL* unsortedAabbs, __global const int* unsortedAabbMapping, __global const int* unsortedAabbMapping2, volatile __global int4* pairsOut,volatile __global int* pairCount, int numUnsortedAabbs, int numUnSortedAabbs2, int axis, int maxPairs) +{ + int i = get_global_id(0); + if (i>=numUnsortedAabbs) + return; + + int j = get_global_id(1); + if (j>=numUnSortedAabbs2) + return; + + + __global const btAabbCL* unsortedAabbPtr = &unsortedAabbs[unsortedAabbMapping[i]]; + __global const btAabbCL* unsortedAabbPtr2 = &unsortedAabbs[unsortedAabbMapping2[j]]; + + if (TestAabbAgainstAabb2GlobalGlobal(unsortedAabbPtr,unsortedAabbPtr2)) + { + int4 myPair; + + int xIndex = unsortedAabbPtr[0].m_minIndices[3]; + int yIndex = unsortedAabbPtr2[0].m_minIndices[3]; + if (xIndex>yIndex) + { + int tmp = xIndex; + xIndex=yIndex; + yIndex=tmp; + } + + myPair.x = xIndex; + myPair.y = yIndex; + myPair.z = NEW_PAIR_MARKER; + myPair.w = NEW_PAIR_MARKER; + + + int curPair = atomic_inc (pairCount); + if (curPair=numObjects) + return; + for (int j=i+1;j=numObjects) + return; + for (int j=i+1;j=numObjects && !localBreak) + { + atomic_inc(breakRequest); + localBreak = 1; + } + barrier(CLK_LOCAL_MEM_FENCE); + + if (!localBreak) + { + if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j])) + { + int4 myPair; + myPair.x = aabbs[i].m_minIndices[3]; + myPair.y = aabbs[j].m_minIndices[3]; + myPair.z = NEW_PAIR_MARKER; + myPair.w = NEW_PAIR_MARKER; + + int curPair = atomic_inc (pairCount); + if (curPair=numObjects && !localBreak) + { + atomic_inc(breakRequest); + localBreak = 1; + } + barrier(CLK_LOCAL_MEM_FENCE); + + if (!localBreak) + { + if (TestAabbAgainstAabb2(&myAabb,&localAabbs[localCount+localId+1])) + { + int4 myPair; + myPair.x = myAabb.m_minIndices[3]; + myPair.y = localAabbs[localCount+localId+1].m_minIndices[3]; + myPair.z = NEW_PAIR_MARKER; + myPair.w = NEW_PAIR_MARKER; + + int curPair = atomic_inc (pairCount); + if (curPair> 31) | 0x80000000; + return f ^ mask; +} +float IFloatFlip(unsigned int f); +float IFloatFlip(unsigned int f) +{ + unsigned int mask = ((f >> 31) - 1) | 0x80000000; + unsigned int fl = f ^ mask; + return *(float*)&fl; +} + + + + +__kernel void copyAabbsKernel( __global const btAabbCL* allAabbs, __global btAabbCL* destAabbs, int numObjects) +{ + int i = get_global_id(0); + if (i>=numObjects) + return; + int src = destAabbs[i].m_maxIndices[3]; + destAabbs[i] = allAabbs[src]; + destAabbs[i].m_maxIndices[3] = src; +} + + +__kernel void flipFloatKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, __global int2* sortData, int numObjects, int axis) +{ + int i = get_global_id(0); + if (i>=numObjects) + return; + + + sortData[i].x = FloatFlip(allAabbs[smallAabbMapping[i]].m_minElems[axis]); + sortData[i].y = i; + +} + + +__kernel void scatterKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, volatile __global const int2* sortData, __global btAabbCL* sortedAabbs, int numObjects) +{ + int i = get_global_id(0); + if (i>=numObjects) + return; + + sortedAabbs[i] = allAabbs[smallAabbMapping[sortData[i].y]]; +} + + + +__kernel void prepareSumVarianceKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, __global float4* sum, __global float4* sum2,int numAabbs) +{ + int i = get_global_id(0); + if (i>=numAabbs) + return; + + btAabbCL smallAabb = allAabbs[smallAabbMapping[i]]; + + float4 s; + s = (smallAabb.m_max+smallAabb.m_min)*0.5f; + sum[i]=s; + sum2[i]=s*s; +} diff --git a/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/sapKernels.h b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/sapKernels.h new file mode 100644 index 000000000000..04d40fcf26ed --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/BroadphaseCollision/kernels/sapKernels.h @@ -0,0 +1,342 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* sapCL= \ +"/*\n" +"Copyright (c) 2012 Advanced Micro Devices, Inc. \n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose, \n" +"including commercial applications, and to alter it and redistribute it freely, \n" +"subject to the following restrictions:\n" +"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.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"//Originally written by Erwin Coumans\n" +"#define NEW_PAIR_MARKER -1\n" +"typedef struct \n" +"{\n" +" union\n" +" {\n" +" float4 m_min;\n" +" float m_minElems[4];\n" +" int m_minIndices[4];\n" +" };\n" +" union\n" +" {\n" +" float4 m_max;\n" +" float m_maxElems[4];\n" +" int m_maxIndices[4];\n" +" };\n" +"} btAabbCL;\n" +"/// conservative test for overlap between two aabbs\n" +"bool TestAabbAgainstAabb2(const btAabbCL* aabb1, __local const btAabbCL* aabb2);\n" +"bool TestAabbAgainstAabb2(const btAabbCL* aabb1, __local const btAabbCL* aabb2)\n" +"{\n" +" bool overlap = true;\n" +" overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n" +" overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n" +" overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n" +" return overlap;\n" +"}\n" +"bool TestAabbAgainstAabb2GlobalGlobal(__global const btAabbCL* aabb1, __global const btAabbCL* aabb2);\n" +"bool TestAabbAgainstAabb2GlobalGlobal(__global const btAabbCL* aabb1, __global const btAabbCL* aabb2)\n" +"{\n" +" bool overlap = true;\n" +" overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n" +" overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n" +" overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n" +" return overlap;\n" +"}\n" +"bool TestAabbAgainstAabb2Global(const btAabbCL* aabb1, __global const btAabbCL* aabb2);\n" +"bool TestAabbAgainstAabb2Global(const btAabbCL* aabb1, __global const btAabbCL* aabb2)\n" +"{\n" +" bool overlap = true;\n" +" overlap = (aabb1->m_min.x > aabb2->m_max.x || aabb1->m_max.x < aabb2->m_min.x) ? false : overlap;\n" +" overlap = (aabb1->m_min.z > aabb2->m_max.z || aabb1->m_max.z < aabb2->m_min.z) ? false : overlap;\n" +" overlap = (aabb1->m_min.y > aabb2->m_max.y || aabb1->m_max.y < aabb2->m_min.y) ? false : overlap;\n" +" return overlap;\n" +"}\n" +"__kernel void computePairsKernelTwoArrays( __global const btAabbCL* unsortedAabbs, __global const int* unsortedAabbMapping, __global const int* unsortedAabbMapping2, volatile __global int4* pairsOut,volatile __global int* pairCount, int numUnsortedAabbs, int numUnSortedAabbs2, int axis, int maxPairs)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numUnsortedAabbs)\n" +" return;\n" +" int j = get_global_id(1);\n" +" if (j>=numUnSortedAabbs2)\n" +" return;\n" +" __global const btAabbCL* unsortedAabbPtr = &unsortedAabbs[unsortedAabbMapping[i]];\n" +" __global const btAabbCL* unsortedAabbPtr2 = &unsortedAabbs[unsortedAabbMapping2[j]];\n" +" if (TestAabbAgainstAabb2GlobalGlobal(unsortedAabbPtr,unsortedAabbPtr2))\n" +" {\n" +" int4 myPair;\n" +" \n" +" int xIndex = unsortedAabbPtr[0].m_minIndices[3];\n" +" int yIndex = unsortedAabbPtr2[0].m_minIndices[3];\n" +" if (xIndex>yIndex)\n" +" {\n" +" int tmp = xIndex;\n" +" xIndex=yIndex;\n" +" yIndex=tmp;\n" +" }\n" +" \n" +" myPair.x = xIndex;\n" +" myPair.y = yIndex;\n" +" myPair.z = NEW_PAIR_MARKER;\n" +" myPair.w = NEW_PAIR_MARKER;\n" +" int curPair = atomic_inc (pairCount);\n" +" if (curPair=numObjects)\n" +" return;\n" +" for (int j=i+1;j=numObjects)\n" +" return;\n" +" for (int j=i+1;j=numObjects && !localBreak)\n" +" {\n" +" atomic_inc(breakRequest);\n" +" localBreak = 1;\n" +" }\n" +" barrier(CLK_LOCAL_MEM_FENCE);\n" +" \n" +" if (!localBreak)\n" +" {\n" +" if (TestAabbAgainstAabb2GlobalGlobal(&aabbs[i],&aabbs[j]))\n" +" {\n" +" int4 myPair;\n" +" myPair.x = aabbs[i].m_minIndices[3];\n" +" myPair.y = aabbs[j].m_minIndices[3];\n" +" myPair.z = NEW_PAIR_MARKER;\n" +" myPair.w = NEW_PAIR_MARKER;\n" +" int curPair = atomic_inc (pairCount);\n" +" if (curPair=numObjects && !localBreak)\n" +" {\n" +" atomic_inc(breakRequest);\n" +" localBreak = 1;\n" +" }\n" +" barrier(CLK_LOCAL_MEM_FENCE);\n" +" \n" +" if (!localBreak)\n" +" {\n" +" if (TestAabbAgainstAabb2(&myAabb,&localAabbs[localCount+localId+1]))\n" +" {\n" +" int4 myPair;\n" +" myPair.x = myAabb.m_minIndices[3];\n" +" myPair.y = localAabbs[localCount+localId+1].m_minIndices[3];\n" +" myPair.z = NEW_PAIR_MARKER;\n" +" myPair.w = NEW_PAIR_MARKER;\n" +" int curPair = atomic_inc (pairCount);\n" +" if (curPair> 31) | 0x80000000;\n" +" return f ^ mask;\n" +"}\n" +"float IFloatFlip(unsigned int f);\n" +"float IFloatFlip(unsigned int f)\n" +"{\n" +" unsigned int mask = ((f >> 31) - 1) | 0x80000000;\n" +" unsigned int fl = f ^ mask;\n" +" return *(float*)&fl;\n" +"}\n" +"__kernel void copyAabbsKernel( __global const btAabbCL* allAabbs, __global btAabbCL* destAabbs, int numObjects)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numObjects)\n" +" return;\n" +" int src = destAabbs[i].m_maxIndices[3];\n" +" destAabbs[i] = allAabbs[src];\n" +" destAabbs[i].m_maxIndices[3] = src;\n" +"}\n" +"__kernel void flipFloatKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, __global int2* sortData, int numObjects, int axis)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numObjects)\n" +" return;\n" +" \n" +" \n" +" sortData[i].x = FloatFlip(allAabbs[smallAabbMapping[i]].m_minElems[axis]);\n" +" sortData[i].y = i;\n" +" \n" +"}\n" +"__kernel void scatterKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, volatile __global const int2* sortData, __global btAabbCL* sortedAabbs, int numObjects)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numObjects)\n" +" return;\n" +" \n" +" sortedAabbs[i] = allAabbs[smallAabbMapping[sortData[i].y]];\n" +"}\n" +"__kernel void prepareSumVarianceKernel( __global const btAabbCL* allAabbs, __global const int* smallAabbMapping, __global float4* sum, __global float4* sum2,int numAabbs)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numAabbs)\n" +" return;\n" +" \n" +" btAabbCL smallAabb = allAabbs[smallAabbMapping[i]];\n" +" \n" +" float4 s;\n" +" s = (smallAabb.m_max+smallAabb.m_min)*0.5f;\n" +" sum[i]=s;\n" +" sum2[i]=s*s; \n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/CMakeLists.txt b/extern/bullet/src/Bullet3OpenCL/CMakeLists.txt new file mode 100644 index 000000000000..1da58d4a99aa --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/CMakeLists.txt @@ -0,0 +1,77 @@ +INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src ) + +ADD_DEFINITIONS(-DB3_USE_CLEW) + +SET(Bullet3OpenCL_clew_SRCS + ../clew/clew.c + BroadphaseCollision/b3GpuGridBroadphase.cpp + BroadphaseCollision/b3GpuSapBroadphase.cpp + BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.cpp + BroadphaseCollision/b3GpuParallelLinearBvh.cpp + Initialize/b3OpenCLUtils.cpp + NarrowphaseCollision/b3ContactCache.cpp + NarrowphaseCollision/b3ConvexHullContact.cpp + NarrowphaseCollision/b3GjkEpa.cpp + NarrowphaseCollision/b3OptimizedBvh.cpp + NarrowphaseCollision/b3QuantizedBvh.cpp + NarrowphaseCollision/b3StridingMeshInterface.cpp + NarrowphaseCollision/b3TriangleCallback.cpp + NarrowphaseCollision/b3TriangleIndexVertexArray.cpp + NarrowphaseCollision/b3VoronoiSimplexSolver.cpp + ParallelPrimitives/b3BoundSearchCL.cpp + ParallelPrimitives/b3FillCL.cpp + ParallelPrimitives/b3LauncherCL.cpp + ParallelPrimitives/b3PrefixScanCL.cpp + ParallelPrimitives/b3PrefixScanFloat4CL.cpp + ParallelPrimitives/b3RadixSort32CL.cpp + Raycast/b3GpuRaycast.cpp + RigidBody/b3GpuGenericConstraint.cpp + RigidBody/b3GpuJacobiContactSolver.cpp + RigidBody/b3GpuNarrowPhase.cpp + RigidBody/b3GpuPgsConstraintSolver.cpp + RigidBody/b3GpuPgsContactSolver.cpp + RigidBody/b3GpuRigidBodyPipeline.cpp + RigidBody/b3Solver.cpp +) + + +SET(Bullet3OpenCL_clew_HDRS +# ${Root_HDRS} +) + + +ADD_LIBRARY(Bullet3OpenCL_clew ${Bullet3OpenCL_clew_SRCS} ${Bullet3OpenCL_clew_HDRS}) +SET_TARGET_PROPERTIES(Bullet3OpenCL_clew PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(Bullet3OpenCL_clew PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + TARGET_LINK_LIBRARIES(Bullet3OpenCL_clew LinearMath Bullet3Dynamics ${CMAKE_DL_LIBS}) +ENDIF (BUILD_SHARED_LIBS) + + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + #INSTALL of other files requires CMake 2.6 + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS Bullet3OpenCL_clew DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS Bullet3OpenCL_clew RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) +# INSTALL(FILES ../btBullet3OpenCL_clewCommon.h +#DESTINATION ${INCLUDE_INSTALL_DIR}/Bullet3OpenCL_clew) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(Bullet3OpenCL_clew PROPERTIES FRAMEWORK true) + + SET_TARGET_PROPERTIES(Bullet3OpenCL_clew PROPERTIES PUBLIC_HEADER "${Root_HDRS}") + # Have to list out sub-directories manually: + SET_PROPERTY(SOURCE ${BroadphaseCollision_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/BroadphaseCollision) + + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/extern/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLInclude.h b/extern/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLInclude.h new file mode 100644 index 000000000000..e79182d7cb88 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLInclude.h @@ -0,0 +1,48 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011 Advanced Micro Devices, Inc. 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. +*/ + +#ifndef B3_OPENCL_INCLUDE_H +#define B3_OPENCL_INCLUDE_H + +#ifdef B3_USE_CLEW + #include "clew/clew.h" +#else + +#ifdef __APPLE__ +#ifdef USE_MINICL +#include +#else +#include +#include //clLogMessagesToStderrAPPLE +#endif +#else +#ifdef USE_MINICL +#include +#else +#include +#ifdef _WIN32 +#include "CL/cl_gl.h" +#endif //_WIN32 +#endif +#endif //__APPLE__ +#endif //B3_USE_CLEW + +#include +#include +#define oclCHECKERROR(a, b) if((a)!=(b)) { printf("OCL Error : %d\n", (a)); assert((a) == (b)); } + + +#endif //B3_OPENCL_INCLUDE_H + diff --git a/extern/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp b/extern/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp new file mode 100644 index 000000000000..896191c89c9e --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp @@ -0,0 +1,1011 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006 - 2011 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. +*/ + +//Original author: Roman Ponomarev +//Mostly Reimplemented by Erwin Coumans + + +bool gDebugForceLoadingFromSource = false; +bool gDebugSkipLoadingBinary = false; + +#include "Bullet3Common/b3Logging.h" + +#include + +#ifdef _WIN32 +#pragma warning (disable:4996) +#endif +#include "b3OpenCLUtils.h" +//#include "b3OpenCLInclude.h" + +#include +#include + +#define B3_MAX_CL_DEVICES 16 //who needs 16 devices? + +#ifdef _WIN32 +#include +#endif + +#include +#define b3Assert assert +#ifndef _WIN32 +#include + +#endif + +static const char* sCachedBinaryPath="cache"; + + +//Set the preferred platform vendor using the OpenCL SDK +static const char* spPlatformVendor = +#if defined(CL_PLATFORM_MINI_CL) +"MiniCL, SCEA"; +#elif defined(CL_PLATFORM_AMD) +"Advanced Micro Devices, Inc."; +#elif defined(CL_PLATFORM_NVIDIA) +"NVIDIA Corporation"; +#elif defined(CL_PLATFORM_INTEL) +"Intel(R) Corporation"; +#elif defined(B3_USE_CLEW) +"clew (OpenCL Extension Wrangler library)"; +#else +"Unknown Vendor"; +#endif + +#ifndef CL_PLATFORM_MINI_CL +#ifdef _WIN32 +#ifndef B3_USE_CLEW +#include "CL/cl_gl.h" +#endif //B3_USE_CLEW +#endif //_WIN32 +#endif + + +void MyFatalBreakAPPLE( const char * errstr , + const void * private_info , + size_t cb , + void * user_data ) +{ + + + const char* patloc = strstr(errstr, "Warning"); + //find out if it is a warning or error, exit if error + + if (patloc) + { + b3Warning("Warning: %s\n", errstr); + } else + { + b3Error("Error: %s\n", errstr); + b3Assert(0); + } + +} + +#ifdef B3_USE_CLEW + +int b3OpenCLUtils_clewInit() +{ + int result = -1; + +#ifdef _WIN32 + const char* cl = "OpenCL.dll"; +#elif defined __APPLE__ + const char* cl = "/System/Library/Frameworks/OpenCL.framework/Versions/Current/OpenCL"; +#else//presumable Linux? + //linux (tested on Ubuntu 12.10 with Catalyst 13.4 beta drivers, not that there is no symbolic link from libOpenCL.so + const char* cl = "libOpenCL.so.1"; + result = clewInit(cl); + if (result != CLEW_SUCCESS) + { + cl = "libOpenCL.so"; + } else + { + clewExit(); + } +#endif + result = clewInit(cl); + if (result!=CLEW_SUCCESS) + { + b3Error("clewInit failed with error code %d\n",result); + } + else + { + b3Printf("clewInit succesfull using %s\n",cl); + } + return result; +} +#endif + +int b3OpenCLUtils_getNumPlatforms(cl_int* pErrNum) +{ +#ifdef B3_USE_CLEW + b3OpenCLUtils_clewInit(); +#endif + + cl_platform_id pPlatforms[10] = { 0 }; + + cl_uint numPlatforms = 0; + cl_int ciErrNum = clGetPlatformIDs(10, pPlatforms, &numPlatforms); + //cl_int ciErrNum = clGetPlatformIDs(0, NULL, &numPlatforms); + + if(ciErrNum != CL_SUCCESS) + { + if(pErrNum != NULL) + *pErrNum = ciErrNum; + } + return numPlatforms; + +} + +const char* b3OpenCLUtils_getSdkVendorName() +{ + return spPlatformVendor; +} + +void b3OpenCLUtils_setCachePath(const char* path) +{ + sCachedBinaryPath = path; +} + +cl_platform_id b3OpenCLUtils_getPlatform(int platformIndex0, cl_int* pErrNum) +{ +#ifdef B3_USE_CLEW + b3OpenCLUtils_clewInit(); +#endif + + cl_platform_id platform = 0; + unsigned int platformIndex = (unsigned int )platformIndex0; + cl_uint numPlatforms; + cl_int ciErrNum = clGetPlatformIDs(0, NULL, &numPlatforms); + + if (platformIndexm_platformVendor,NULL); + oclCHECKERROR(ciErrNum,CL_SUCCESS); + ciErrNum = clGetPlatformInfo( platform,CL_PLATFORM_NAME,B3_MAX_STRING_LENGTH,platformInfo->m_platformName,NULL); + oclCHECKERROR(ciErrNum,CL_SUCCESS); + ciErrNum = clGetPlatformInfo( platform,CL_PLATFORM_VERSION,B3_MAX_STRING_LENGTH,platformInfo->m_platformVersion,NULL); + oclCHECKERROR(ciErrNum,CL_SUCCESS); +} + +void b3OpenCLUtils_printPlatformInfo( cl_platform_id platform) +{ + b3OpenCLPlatformInfo platformInfo; + b3OpenCLUtils::getPlatformInfo (platform, &platformInfo); + b3Printf("Platform info:\n"); + b3Printf(" CL_PLATFORM_VENDOR: \t\t\t%s\n",platformInfo.m_platformVendor); + b3Printf(" CL_PLATFORM_NAME: \t\t\t%s\n",platformInfo.m_platformName); + b3Printf(" CL_PLATFORM_VERSION: \t\t\t%s\n",platformInfo.m_platformVersion); +} + + + +cl_context b3OpenCLUtils_createContextFromPlatform(cl_platform_id platform, cl_device_type deviceType, cl_int* pErrNum, void* pGLContext, void* pGLDC, int preferredDeviceIndex, int preferredPlatformIndex) +{ + cl_context retContext = 0; + cl_int ciErrNum=0; + cl_uint num_entries; + cl_device_id devices[B3_MAX_CL_DEVICES]; + cl_uint num_devices; + cl_context_properties* cprops; + + /* + * If we could find our platform, use it. Otherwise pass a NULL and get whatever the + * implementation thinks we should be using. + */ + cl_context_properties cps[7] = {0,0,0,0,0,0,0}; + cps[0] = CL_CONTEXT_PLATFORM; + cps[1] = (cl_context_properties)platform; +#ifdef _WIN32 +#ifndef B3_USE_CLEW + if (pGLContext && pGLDC) + { + cps[2] = CL_GL_CONTEXT_KHR; + cps[3] = (cl_context_properties)pGLContext; + cps[4] = CL_WGL_HDC_KHR; + cps[5] = (cl_context_properties)pGLDC; + } +#endif //B3_USE_CLEW +#endif //_WIN32 + num_entries = B3_MAX_CL_DEVICES; + + + num_devices=-1; + + ciErrNum = clGetDeviceIDs( + platform, + deviceType, + num_entries, + devices, + &num_devices); + + if (ciErrNum<0) + { + b3Printf("clGetDeviceIDs returned %d\n",ciErrNum); + return 0; + } + cprops = (NULL == platform) ? NULL : cps; + + if (!num_devices) + return 0; + + if (pGLContext) + { + //search for the GPU that relates to the OpenCL context + unsigned int i; + for (i=0;i=0 && (unsigned int)preferredDeviceIndex 0) + { + cl_platform_id* platforms = (cl_platform_id*) malloc (sizeof(cl_platform_id)*numPlatforms); + ciErrNum = clGetPlatformIDs(numPlatforms, platforms, NULL); + if(ciErrNum != CL_SUCCESS) + { + if(pErrNum != NULL) + *pErrNum = ciErrNum; + free(platforms); + return NULL; + } + + + + for ( i = 0; i < numPlatforms; ++i) + { + char pbuf[128]; + ciErrNum = clGetPlatformInfo( platforms[i], + CL_PLATFORM_VENDOR, + sizeof(pbuf), + pbuf, + NULL); + if(ciErrNum != CL_SUCCESS) + { + if(pErrNum != NULL) *pErrNum = ciErrNum; + return NULL; + } + + if (preferredPlatformIndex>=0 && i==preferredPlatformIndex) + { + cl_platform_id tmpPlatform = platforms[0]; + platforms[0] = platforms[i]; + platforms[i] = tmpPlatform; + break; + } else + { + if(!strcmp(pbuf, spPlatformVendor)) + { + cl_platform_id tmpPlatform = platforms[0]; + platforms[0] = platforms[i]; + platforms[i] = tmpPlatform; + } + } + } + + for (i = 0; i < numPlatforms; ++i) + { + cl_platform_id platform = platforms[i]; + assert(platform); + + retContext = b3OpenCLUtils_createContextFromPlatform(platform,deviceType,pErrNum,pGLContext,pGLDC,preferredDeviceIndex,preferredPlatformIndex); + + if (retContext) + { +// printf("OpenCL platform details:\n"); + b3OpenCLPlatformInfo platformInfo; + + b3OpenCLUtils::getPlatformInfo(platform, &platformInfo); + + if (retPlatformId) + *retPlatformId = platform; + + break; + } + } + + free (platforms); + } + return retContext; +} + + +////////////////////////////////////////////////////////////////////////////// +//! Gets the id of the nth device from the context +//! +//! @return the id or -1 when out of range +//! @param cxMainContext OpenCL context +//! @param device_idx index of the device of interest +////////////////////////////////////////////////////////////////////////////// +cl_device_id b3OpenCLUtils_getDevice(cl_context cxMainContext, int deviceIndex) +{ + assert(cxMainContext); + + size_t szParmDataBytes; + cl_device_id* cdDevices; + cl_device_id device ; + + // get the list of devices associated with context + clGetContextInfo(cxMainContext, CL_CONTEXT_DEVICES, 0, NULL, &szParmDataBytes); + + if( szParmDataBytes / sizeof(cl_device_id) < (unsigned int)deviceIndex ) { + return (cl_device_id)-1; + } + + cdDevices = (cl_device_id*) malloc(szParmDataBytes); + + clGetContextInfo(cxMainContext, CL_CONTEXT_DEVICES, szParmDataBytes, cdDevices, NULL); + + device = cdDevices[deviceIndex]; + free(cdDevices); + + return device; +} + +int b3OpenCLUtils_getNumDevices(cl_context cxMainContext) +{ + size_t szParamDataBytes; + int device_count; + clGetContextInfo(cxMainContext, CL_CONTEXT_DEVICES, 0, NULL, &szParamDataBytes); + device_count = (int) szParamDataBytes/ sizeof(cl_device_id); + return device_count; +} + + + +void b3OpenCLUtils::getDeviceInfo(cl_device_id device, b3OpenCLDeviceInfo* info) +{ + // CL_DEVICE_NAME + clGetDeviceInfo(device, CL_DEVICE_NAME, B3_MAX_STRING_LENGTH, &info->m_deviceName, NULL); + + // CL_DEVICE_VENDOR + clGetDeviceInfo(device, CL_DEVICE_VENDOR, B3_MAX_STRING_LENGTH, &info->m_deviceVendor, NULL); + + // CL_DRIVER_VERSION + clGetDeviceInfo(device, CL_DRIVER_VERSION, B3_MAX_STRING_LENGTH, &info->m_driverVersion, NULL); + + // CL_DEVICE_INFO + clGetDeviceInfo(device, CL_DEVICE_TYPE, sizeof(cl_device_type), &info->m_deviceType, NULL); + + // CL_DEVICE_MAX_COMPUTE_UNITS + clGetDeviceInfo(device, CL_DEVICE_MAX_COMPUTE_UNITS, sizeof(info->m_computeUnits), &info->m_computeUnits, NULL); + + // CL_DEVICE_MAX_WORK_ITEM_DIMENSIONS + clGetDeviceInfo(device, CL_DEVICE_MAX_WORK_ITEM_DIMENSIONS, sizeof(info->m_workitemDims), &info->m_workitemDims, NULL); + + // CL_DEVICE_MAX_WORK_ITEM_SIZES + clGetDeviceInfo(device, CL_DEVICE_MAX_WORK_ITEM_SIZES, sizeof(info->m_workItemSize), &info->m_workItemSize, NULL); + + // CL_DEVICE_MAX_WORK_GROUP_SIZE + clGetDeviceInfo(device, CL_DEVICE_MAX_WORK_GROUP_SIZE, sizeof(info->m_workgroupSize), &info->m_workgroupSize, NULL); + + // CL_DEVICE_MAX_CLOCK_FREQUENCY + clGetDeviceInfo(device, CL_DEVICE_MAX_CLOCK_FREQUENCY, sizeof(info->m_clockFrequency), &info->m_clockFrequency, NULL); + + // CL_DEVICE_ADDRESS_BITS + clGetDeviceInfo(device, CL_DEVICE_ADDRESS_BITS, sizeof(info->m_addressBits), &info->m_addressBits, NULL); + + // CL_DEVICE_MAX_MEM_ALLOC_SIZE + clGetDeviceInfo(device, CL_DEVICE_MAX_MEM_ALLOC_SIZE, sizeof(info->m_maxMemAllocSize), &info->m_maxMemAllocSize, NULL); + + // CL_DEVICE_GLOBAL_MEM_SIZE + clGetDeviceInfo(device, CL_DEVICE_GLOBAL_MEM_SIZE, sizeof(info->m_globalMemSize), &info->m_globalMemSize, NULL); + + // CL_DEVICE_ERROR_CORRECTION_SUPPORT + clGetDeviceInfo(device, CL_DEVICE_ERROR_CORRECTION_SUPPORT, sizeof(info->m_errorCorrectionSupport), &info->m_errorCorrectionSupport, NULL); + + // CL_DEVICE_LOCAL_MEM_TYPE + clGetDeviceInfo(device, CL_DEVICE_LOCAL_MEM_TYPE, sizeof(info->m_localMemType), &info->m_localMemType, NULL); + + // CL_DEVICE_LOCAL_MEM_SIZE + clGetDeviceInfo(device, CL_DEVICE_LOCAL_MEM_SIZE, sizeof(info->m_localMemSize), &info->m_localMemSize, NULL); + + // CL_DEVICE_MAX_CONSTANT_BUFFER_SIZE + clGetDeviceInfo(device, CL_DEVICE_MAX_CONSTANT_BUFFER_SIZE, sizeof(info->m_constantBufferSize), &info->m_constantBufferSize, NULL); + + // CL_DEVICE_QUEUE_PROPERTIES + clGetDeviceInfo(device, CL_DEVICE_QUEUE_PROPERTIES, sizeof(info->m_queueProperties), &info->m_queueProperties, NULL); + + // CL_DEVICE_IMAGE_SUPPORT + clGetDeviceInfo(device, CL_DEVICE_IMAGE_SUPPORT, sizeof(info->m_imageSupport), &info->m_imageSupport, NULL); + + // CL_DEVICE_MAX_READ_IMAGE_ARGS + clGetDeviceInfo(device, CL_DEVICE_MAX_READ_IMAGE_ARGS, sizeof(info->m_maxReadImageArgs), &info->m_maxReadImageArgs, NULL); + + // CL_DEVICE_MAX_WRITE_IMAGE_ARGS + clGetDeviceInfo(device, CL_DEVICE_MAX_WRITE_IMAGE_ARGS, sizeof(info->m_maxWriteImageArgs), &info->m_maxWriteImageArgs, NULL); + + // CL_DEVICE_IMAGE2D_MAX_WIDTH, CL_DEVICE_IMAGE2D_MAX_HEIGHT, CL_DEVICE_IMAGE3D_MAX_WIDTH, CL_DEVICE_IMAGE3D_MAX_HEIGHT, CL_DEVICE_IMAGE3D_MAX_DEPTH + clGetDeviceInfo(device, CL_DEVICE_IMAGE2D_MAX_WIDTH, sizeof(size_t), &info->m_image2dMaxWidth, NULL); + clGetDeviceInfo(device, CL_DEVICE_IMAGE2D_MAX_HEIGHT, sizeof(size_t), &info->m_image2dMaxHeight, NULL); + clGetDeviceInfo(device, CL_DEVICE_IMAGE3D_MAX_WIDTH, sizeof(size_t), &info->m_image3dMaxWidth, NULL); + clGetDeviceInfo(device, CL_DEVICE_IMAGE3D_MAX_HEIGHT, sizeof(size_t), &info->m_image3dMaxHeight, NULL); + clGetDeviceInfo(device, CL_DEVICE_IMAGE3D_MAX_DEPTH, sizeof(size_t), &info->m_image3dMaxDepth, NULL); + + // CL_DEVICE_EXTENSIONS: get device extensions, and if any then parse & log the string onto separate lines + clGetDeviceInfo(device, CL_DEVICE_EXTENSIONS, B3_MAX_STRING_LENGTH, &info->m_deviceExtensions, NULL); + + // CL_DEVICE_PREFERRED_VECTOR_WIDTH_ + clGetDeviceInfo(device, CL_DEVICE_PREFERRED_VECTOR_WIDTH_CHAR, sizeof(cl_uint), &info->m_vecWidthChar, NULL); + clGetDeviceInfo(device, CL_DEVICE_PREFERRED_VECTOR_WIDTH_SHORT, sizeof(cl_uint), &info->m_vecWidthShort, NULL); + clGetDeviceInfo(device, CL_DEVICE_PREFERRED_VECTOR_WIDTH_INT, sizeof(cl_uint), &info->m_vecWidthInt, NULL); + clGetDeviceInfo(device, CL_DEVICE_PREFERRED_VECTOR_WIDTH_LONG, sizeof(cl_uint), &info->m_vecWidthLong, NULL); + clGetDeviceInfo(device, CL_DEVICE_PREFERRED_VECTOR_WIDTH_FLOAT, sizeof(cl_uint), &info->m_vecWidthFloat, NULL); + clGetDeviceInfo(device, CL_DEVICE_PREFERRED_VECTOR_WIDTH_DOUBLE, sizeof(cl_uint), &info->m_vecWidthDouble, NULL); +} + + +void b3OpenCLUtils_printDeviceInfo(cl_device_id device) +{ + b3OpenCLDeviceInfo info; + b3OpenCLUtils::getDeviceInfo(device,&info); + b3Printf("Device Info:\n"); + b3Printf(" CL_DEVICE_NAME: \t\t\t%s\n", info.m_deviceName); + b3Printf(" CL_DEVICE_VENDOR: \t\t\t%s\n", info.m_deviceVendor); + b3Printf(" CL_DRIVER_VERSION: \t\t\t%s\n", info.m_driverVersion); + + if( info.m_deviceType & CL_DEVICE_TYPE_CPU ) + b3Printf(" CL_DEVICE_TYPE:\t\t\t%s\n", "CL_DEVICE_TYPE_CPU"); + if( info.m_deviceType & CL_DEVICE_TYPE_GPU ) + b3Printf(" CL_DEVICE_TYPE:\t\t\t%s\n", "CL_DEVICE_TYPE_GPU"); + if( info.m_deviceType & CL_DEVICE_TYPE_ACCELERATOR ) + b3Printf(" CL_DEVICE_TYPE:\t\t\t%s\n", "CL_DEVICE_TYPE_ACCELERATOR"); + if( info.m_deviceType & CL_DEVICE_TYPE_DEFAULT ) + b3Printf(" CL_DEVICE_TYPE:\t\t\t%s\n", "CL_DEVICE_TYPE_DEFAULT"); + + b3Printf(" CL_DEVICE_MAX_COMPUTE_UNITS:\t\t%u\n", info.m_computeUnits); + b3Printf(" CL_DEVICE_MAX_WORK_ITEM_DIMENSIONS:\t%u\n", info.m_workitemDims); + b3Printf(" CL_DEVICE_MAX_WORK_ITEM_SIZES:\t%u / %u / %u \n", info.m_workItemSize[0], info.m_workItemSize[1], info.m_workItemSize[2]); + b3Printf(" CL_DEVICE_MAX_WORK_GROUP_SIZE:\t%u\n", info.m_workgroupSize); + b3Printf(" CL_DEVICE_MAX_CLOCK_FREQUENCY:\t%u MHz\n", info.m_clockFrequency); + b3Printf(" CL_DEVICE_ADDRESS_BITS:\t\t%u\n", info.m_addressBits); + b3Printf(" CL_DEVICE_MAX_MEM_ALLOC_SIZE:\t\t%u MByte\n", (unsigned int)(info.m_maxMemAllocSize/ (1024 * 1024))); + b3Printf(" CL_DEVICE_GLOBAL_MEM_SIZE:\t\t%u MByte\n", (unsigned int)(info.m_globalMemSize/ (1024 * 1024))); + b3Printf(" CL_DEVICE_ERROR_CORRECTION_SUPPORT:\t%s\n", info.m_errorCorrectionSupport== CL_TRUE ? "yes" : "no"); + b3Printf(" CL_DEVICE_LOCAL_MEM_TYPE:\t\t%s\n", info.m_localMemType == 1 ? "local" : "global"); + b3Printf(" CL_DEVICE_LOCAL_MEM_SIZE:\t\t%u KByte\n", (unsigned int)(info.m_localMemSize / 1024)); + b3Printf(" CL_DEVICE_MAX_CONSTANT_BUFFER_SIZE:\t%u KByte\n", (unsigned int)(info.m_constantBufferSize / 1024)); + if( info.m_queueProperties & CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE ) + b3Printf(" CL_DEVICE_QUEUE_PROPERTIES:\t\t%s\n", "CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE"); + if( info.m_queueProperties & CL_QUEUE_PROFILING_ENABLE ) + b3Printf(" CL_DEVICE_QUEUE_PROPERTIES:\t\t%s\n", "CL_QUEUE_PROFILING_ENABLE"); + + b3Printf(" CL_DEVICE_IMAGE_SUPPORT:\t\t%u\n", info.m_imageSupport); + + b3Printf(" CL_DEVICE_MAX_READ_IMAGE_ARGS:\t%u\n", info.m_maxReadImageArgs); + b3Printf(" CL_DEVICE_MAX_WRITE_IMAGE_ARGS:\t%u\n", info.m_maxWriteImageArgs); + b3Printf("\n CL_DEVICE_IMAGE "); + b3Printf("\t\t\t2D_MAX_WIDTH\t %u\n", info.m_image2dMaxWidth); + b3Printf("\t\t\t\t\t2D_MAX_HEIGHT\t %u\n", info.m_image2dMaxHeight); + b3Printf("\t\t\t\t\t3D_MAX_WIDTH\t %u\n", info.m_image3dMaxWidth); + b3Printf("\t\t\t\t\t3D_MAX_HEIGHT\t %u\n", info.m_image3dMaxHeight); + b3Printf("\t\t\t\t\t3D_MAX_DEPTH\t %u\n", info.m_image3dMaxDepth); + if (*info.m_deviceExtensions != 0) + { + b3Printf("\n CL_DEVICE_EXTENSIONS:%s\n",info.m_deviceExtensions); + } + else + { + b3Printf(" CL_DEVICE_EXTENSIONS: None\n"); + } + b3Printf(" CL_DEVICE_PREFERRED_VECTOR_WIDTH_\t"); + b3Printf("CHAR %u, SHORT %u, INT %u,LONG %u, FLOAT %u, DOUBLE %u\n\n\n", + info.m_vecWidthChar, info.m_vecWidthShort, info.m_vecWidthInt, info.m_vecWidthLong,info.m_vecWidthFloat, info.m_vecWidthDouble); + + +} + + +static const char* strip2(const char* name, const char* pattern) +{ + size_t const patlen = strlen(pattern); + size_t patcnt = 0; + const char * oriptr; + const char * patloc; + // find how many times the pattern occurs in the original string + for (oriptr = name; (patloc = strstr(oriptr, pattern)); oriptr = patloc + patlen) + { + patcnt++; + } + return oriptr; +} + +cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_device_id device, const char* kernelSourceOrg, cl_int* pErrNum, const char* additionalMacrosArg , const char* clFileNameForCaching, bool disableBinaryCaching) +{ + const char* additionalMacros = additionalMacrosArg?additionalMacrosArg:""; + + if (disableBinaryCaching) + { + //kernelSourceOrg = 0; + } + + cl_program m_cpProgram=0; + cl_int status; + + char binaryFileName[B3_MAX_STRING_LENGTH]; + + char deviceName[256]; + char driverVersion[256]; + const char* strippedName; + int fileUpToDate = 0; +#ifdef _WIN32 + int binaryFileValid=0; +#endif + if (!disableBinaryCaching && clFileNameForCaching) + { + clGetDeviceInfo(device, CL_DEVICE_NAME, 256, &deviceName, NULL); + clGetDeviceInfo(device, CL_DRIVER_VERSION, 256, &driverVersion, NULL); + + strippedName = strip2(clFileNameForCaching,"\\"); + strippedName = strip2(strippedName,"/"); + +#ifdef _MSC_VER + sprintf_s(binaryFileName,B3_MAX_STRING_LENGTH,"%s/%s.%s.%s.bin",sCachedBinaryPath,strippedName, deviceName,driverVersion ); +#else + sprintf(binaryFileName,"%s/%s.%s.%s.bin",sCachedBinaryPath,strippedName, deviceName,driverVersion ); +#endif + } + if (clFileNameForCaching && !(disableBinaryCaching || gDebugSkipLoadingBinary||gDebugForceLoadingFromSource) ) + { + +#ifdef _WIN32 + char* bla=0; + + + + //printf("searching for %s\n", binaryFileName); + + + FILETIME modtimeBinary; + CreateDirectoryA(sCachedBinaryPath,0); + { + + HANDLE binaryFileHandle = CreateFileA(binaryFileName,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0); + if (binaryFileHandle ==INVALID_HANDLE_VALUE) + { + DWORD errorCode; + errorCode = GetLastError(); + switch (errorCode) + { + case ERROR_FILE_NOT_FOUND: + { + b3Warning("\nCached file not found %s\n", binaryFileName); + break; + } + case ERROR_PATH_NOT_FOUND: + { + b3Warning("\nCached file path not found %s\n", binaryFileName); + break; + } + default: + { + b3Warning("\nFailed reading cached file with errorCode = %d\n", errorCode); + } + } + } else + { + if (GetFileTime(binaryFileHandle, NULL, NULL, &modtimeBinary)==0) + { + DWORD errorCode; + errorCode = GetLastError(); + b3Warning("\nGetFileTime errorCode = %d\n", errorCode); + } else + { + binaryFileValid = 1; + } + CloseHandle(binaryFileHandle); + } + + if (binaryFileValid) + { + HANDLE srcFileHandle = CreateFileA(clFileNameForCaching,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0); + + if (srcFileHandle==INVALID_HANDLE_VALUE) + { + const char* prefix[]={"./","../","../../","../../../","../../../../"}; + for (int i=0;(srcFileHandle==INVALID_HANDLE_VALUE) && i<5;i++) + { + char relativeFileName[1024]; + sprintf(relativeFileName,"%s%s",prefix[i],clFileNameForCaching); + srcFileHandle = CreateFileA(relativeFileName,GENERIC_READ,0,0,OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL,0); + } + + } + + + if (srcFileHandle!=INVALID_HANDLE_VALUE) + { + FILETIME modtimeSrc; + if (GetFileTime(srcFileHandle, NULL, NULL, &modtimeSrc)==0) + { + DWORD errorCode; + errorCode = GetLastError(); + b3Warning("\nGetFileTime errorCode = %d\n", errorCode); + } + if ( ( modtimeSrc.dwHighDateTime < modtimeBinary.dwHighDateTime) + ||(( modtimeSrc.dwHighDateTime == modtimeBinary.dwHighDateTime)&&(modtimeSrc.dwLowDateTime <= modtimeBinary.dwLowDateTime))) + { + fileUpToDate=1; + } else + { + b3Warning("\nCached binary file out-of-date (%s)\n",binaryFileName); + } + CloseHandle(srcFileHandle); + } + else + { +#ifdef _DEBUG + DWORD errorCode; + errorCode = GetLastError(); + switch (errorCode) + { + case ERROR_FILE_NOT_FOUND: + { + b3Warning("\nSrc file not found %s\n", clFileNameForCaching); + break; + } + case ERROR_PATH_NOT_FOUND: + { + b3Warning("\nSrc path not found %s\n", clFileNameForCaching); + break; + } + default: + { + b3Warning("\nnSrc file reading errorCode = %d\n", errorCode); + } + } + + //we should make sure the src file exists so we can verify the timestamp with binary +// assert(0); + b3Warning("Warning: cannot find OpenCL kernel %s to verify timestamp of binary cached kernel %s\n",clFileNameForCaching, binaryFileName); + fileUpToDate = true; +#else + //if we cannot find the source, assume it is OK in release builds + fileUpToDate = true; +#endif + } + } + + + } + + + +#else + fileUpToDate = true; + if (mkdir(sCachedBinaryPath,0777) == -1) + { + } + else + { + b3Printf("Succesfully created cache directory: %s\n", sCachedBinaryPath); + } +#endif //_WIN32 + } + + + if( fileUpToDate) + { +#ifdef _MSC_VER + FILE* file; + if (fopen_s(&file,binaryFileName, "rb")!=0) + file=0; +#else + FILE* file = fopen(binaryFileName, "rb"); +#endif + + if (file) + { + size_t binarySize=0; + char* binary =0; + + fseek( file, 0L, SEEK_END ); + binarySize = ftell( file ); + rewind( file ); + binary = (char*)malloc(sizeof(char)*binarySize); + int bytesRead; + bytesRead = fread( binary, sizeof(char), binarySize, file ); + fclose( file ); + + m_cpProgram = clCreateProgramWithBinary( clContext, 1,&device, &binarySize, (const unsigned char**)&binary, 0, &status ); + b3Assert( status == CL_SUCCESS ); + status = clBuildProgram( m_cpProgram, 1, &device, additionalMacros, 0, 0 ); + b3Assert( status == CL_SUCCESS ); + + if( status != CL_SUCCESS ) + { + char *build_log; + size_t ret_val_size; + clGetProgramBuildInfo(m_cpProgram, device, CL_PROGRAM_BUILD_LOG, 0, NULL, &ret_val_size); + build_log = (char*)malloc(sizeof(char)*(ret_val_size+1)); + clGetProgramBuildInfo(m_cpProgram, device, CL_PROGRAM_BUILD_LOG, ret_val_size, build_log, NULL); + build_log[ret_val_size] = '\0'; + b3Error("%s\n", build_log); + free (build_log); + b3Assert(0); + m_cpProgram = 0; + + b3Warning("clBuildProgram reported failure on cached binary: %s\n",binaryFileName); + + } else + { + b3Printf("clBuildProgram successfully compiled cached binary: %s\n",binaryFileName); + } + free (binary); + + } else + { + b3Warning("Cannot open cached binary: %s\n",binaryFileName); + } + } + + + + + + + + + + if (!m_cpProgram) + { + + cl_int localErrNum; + char* compileFlags; + int flagsize; + + + + const char* kernelSource = kernelSourceOrg; + + if (!kernelSourceOrg || gDebugForceLoadingFromSource) + { + if (clFileNameForCaching) + { + + FILE* file = fopen(clFileNameForCaching, "rb"); + //in many cases the relative path is a few levels up the directory hierarchy, so try it + if (!file) + { + const char* prefix[]={"../","../../","../../../","../../../../"}; + for (int i=0;!file && i<3;i++) + { + char relativeFileName[1024]; + sprintf(relativeFileName,"%s%s",prefix[i],clFileNameForCaching); + file = fopen(relativeFileName, "rb"); + } + } + + if (file) + { + char* kernelSrc=0; + fseek( file, 0L, SEEK_END ); + int kernelSize = ftell( file ); + rewind( file ); + kernelSrc = (char*)malloc(kernelSize+1); + int readBytes; + readBytes = fread((void*)kernelSrc,1,kernelSize, file); + kernelSrc[kernelSize] = 0; + fclose(file); + kernelSource = kernelSrc; + } + } + } + + size_t program_length = kernelSource ? strlen(kernelSource) : 0; +#ifdef MAC //or __APPLE__? + char* flags = "-cl-mad-enable -DMAC "; +#else + const char* flags = ""; +#endif + + + m_cpProgram = clCreateProgramWithSource(clContext, 1, (const char**)&kernelSource, &program_length, &localErrNum); + if (localErrNum!= CL_SUCCESS) + { + if (pErrNum) + *pErrNum = localErrNum; + return 0; + } + + // Build the program with 'mad' Optimization option + + + + flagsize = sizeof(char)*(strlen(additionalMacros) + strlen(flags) + 5); + compileFlags = (char*) malloc(flagsize); +#ifdef _MSC_VER + sprintf_s(compileFlags,flagsize, "%s %s", flags, additionalMacros); +#else + sprintf(compileFlags, "%s %s", flags, additionalMacros); +#endif + localErrNum = clBuildProgram(m_cpProgram, 1, &device, compileFlags, NULL, NULL); + if (localErrNum!= CL_SUCCESS) + { + char *build_log; + size_t ret_val_size; + clGetProgramBuildInfo(m_cpProgram, device, CL_PROGRAM_BUILD_LOG, 0, NULL, &ret_val_size); + build_log = (char*) malloc(sizeof(char)*(ret_val_size+1)); + clGetProgramBuildInfo(m_cpProgram, device, CL_PROGRAM_BUILD_LOG, ret_val_size, build_log, NULL); + + // to be carefully, terminate with \0 + // there's no information in the reference whether the string is 0 terminated or not + build_log[ret_val_size] = '\0'; + + + b3Error("Error in clBuildProgram, Line %u in file %s, Log: \n%s\n !!!\n\n", __LINE__, __FILE__, build_log); + free (build_log); + if (pErrNum) + *pErrNum = localErrNum; + return 0; + } + + + if( !disableBinaryCaching && clFileNameForCaching ) + { // write to binary + + cl_uint numAssociatedDevices; + status = clGetProgramInfo( m_cpProgram, CL_PROGRAM_NUM_DEVICES, sizeof(cl_uint), &numAssociatedDevices, 0 ); + b3Assert( status == CL_SUCCESS ); + if (numAssociatedDevices==1) + { + + size_t binarySize; + char* binary ; + + status = clGetProgramInfo( m_cpProgram, CL_PROGRAM_BINARY_SIZES, sizeof(size_t), &binarySize, 0 ); + b3Assert( status == CL_SUCCESS ); + + binary = (char*)malloc(sizeof(char)*binarySize); + + status = clGetProgramInfo( m_cpProgram, CL_PROGRAM_BINARIES, sizeof(char*), &binary, 0 ); + b3Assert( status == CL_SUCCESS ); + + { + FILE* file=0; +#ifdef _MSC_VER + if (fopen_s(&file,binaryFileName, "wb")!=0) + file=0; +#else + file = fopen(binaryFileName, "wb"); +#endif + if (file) + { + fwrite( binary, sizeof(char), binarySize, file ); + fclose( file ); + } else + { + b3Warning("cannot write file %s\n", binaryFileName); + } + } + + free (binary); + } + } + + free(compileFlags); + + } + return m_cpProgram; +} + + +cl_kernel b3OpenCLUtils_compileCLKernelFromString(cl_context clContext, cl_device_id device, const char* kernelSource, const char* kernelName, cl_int* pErrNum, cl_program prog, const char* additionalMacros ) +{ + + cl_kernel kernel; + cl_int localErrNum; + + cl_program m_cpProgram = prog; + + b3Printf("compiling kernel %s ",kernelName); + + if (!m_cpProgram) + { + m_cpProgram = b3OpenCLUtils_compileCLProgramFromString(clContext,device,kernelSource,pErrNum, additionalMacros,0, false); + } + + + // Create the kernel + kernel = clCreateKernel(m_cpProgram, kernelName, &localErrNum); + if (localErrNum != CL_SUCCESS) + { + b3Error("Error in clCreateKernel, Line %u in file %s, cannot find kernel function %s !!!\n\n", __LINE__, __FILE__, kernelName); + assert(0); + if (pErrNum) + *pErrNum = localErrNum; + return 0; + } + + if (!prog && m_cpProgram) + { + clReleaseProgram(m_cpProgram); + } + b3Printf("ready. \n"); + + + if (pErrNum) + *pErrNum = CL_SUCCESS; + return kernel; + +} diff --git a/extern/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.h b/extern/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.h new file mode 100644 index 000000000000..db6466e76b20 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.h @@ -0,0 +1,194 @@ +/* +Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org +Copyright (C) 2006 - 2011 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. +*/ + +//original author: Roman Ponomarev +//cleanup by Erwin Coumans + +#ifndef B3_OPENCL_UTILS_H +#define B3_OPENCL_UTILS_H + +#include "b3OpenCLInclude.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +///C API for OpenCL utilities: convenience functions, see below for C++ API + +/// CL Context optionally takes a GL context. This is a generic type because we don't really want this code +/// to have to understand GL types. It is a HGLRC in _WIN32 or a GLXContext otherwise. +cl_context b3OpenCLUtils_createContextFromType(cl_device_type deviceType, cl_int* pErrNum, void* pGLCtx , void* pGLDC , int preferredDeviceIndex , int preferredPlatformIndex, cl_platform_id* platformId); + +int b3OpenCLUtils_getNumDevices(cl_context cxMainContext); + +cl_device_id b3OpenCLUtils_getDevice(cl_context cxMainContext, int nr); + +void b3OpenCLUtils_printDeviceInfo(cl_device_id device); + +cl_kernel b3OpenCLUtils_compileCLKernelFromString( cl_context clContext,cl_device_id device, const char* kernelSource, const char* kernelName, cl_int* pErrNum, cl_program prog,const char* additionalMacros); + +//optional +cl_program b3OpenCLUtils_compileCLProgramFromString( cl_context clContext,cl_device_id device, const char* kernelSource, cl_int* pErrNum,const char* additionalMacros , const char* srcFileNameForCaching, bool disableBinaryCaching); + +//the following optional APIs provide access using specific platform information +int b3OpenCLUtils_getNumPlatforms(cl_int* pErrNum); + +///get the nr'th platform, where nr is in the range [0..getNumPlatforms) +cl_platform_id b3OpenCLUtils_getPlatform(int nr, cl_int* pErrNum); + + +void b3OpenCLUtils_printPlatformInfo(cl_platform_id platform); + +const char* b3OpenCLUtils_getSdkVendorName(); + +///set the path (directory/folder) where the compiled OpenCL kernel are stored +void b3OpenCLUtils_setCachePath(const char* path); + +cl_context b3OpenCLUtils_createContextFromPlatform(cl_platform_id platform, cl_device_type deviceType, cl_int* pErrNum, void* pGLCtx , void* pGLDC ,int preferredDeviceIndex , int preferredPlatformIndex); + +#ifdef __cplusplus +} + +#define B3_MAX_STRING_LENGTH 1024 + +typedef struct +{ + char m_deviceName[B3_MAX_STRING_LENGTH]; + char m_deviceVendor[B3_MAX_STRING_LENGTH]; + char m_driverVersion[B3_MAX_STRING_LENGTH]; + char m_deviceExtensions[B3_MAX_STRING_LENGTH]; + + cl_device_type m_deviceType; + cl_uint m_computeUnits; + size_t m_workitemDims; + size_t m_workItemSize[3]; + size_t m_image2dMaxWidth; + size_t m_image2dMaxHeight; + size_t m_image3dMaxWidth; + size_t m_image3dMaxHeight; + size_t m_image3dMaxDepth; + size_t m_workgroupSize; + cl_uint m_clockFrequency; + cl_ulong m_constantBufferSize; + cl_ulong m_localMemSize; + cl_ulong m_globalMemSize; + cl_bool m_errorCorrectionSupport; + cl_device_local_mem_type m_localMemType; + cl_uint m_maxReadImageArgs; + cl_uint m_maxWriteImageArgs; + + + + cl_uint m_addressBits; + cl_ulong m_maxMemAllocSize; + cl_command_queue_properties m_queueProperties; + cl_bool m_imageSupport; + cl_uint m_vecWidthChar; + cl_uint m_vecWidthShort; + cl_uint m_vecWidthInt; + cl_uint m_vecWidthLong; + cl_uint m_vecWidthFloat; + cl_uint m_vecWidthDouble; + +} b3OpenCLDeviceInfo; + +struct b3OpenCLPlatformInfo +{ + char m_platformVendor[B3_MAX_STRING_LENGTH]; + char m_platformName[B3_MAX_STRING_LENGTH]; + char m_platformVersion[B3_MAX_STRING_LENGTH]; + + b3OpenCLPlatformInfo() + { + m_platformVendor[0]=0; + m_platformName[0]=0; + m_platformVersion[0]=0; + } +}; + + +///C++ API for OpenCL utilities: convenience functions +struct b3OpenCLUtils +{ + /// CL Context optionally takes a GL context. This is a generic type because we don't really want this code + /// to have to understand GL types. It is a HGLRC in _WIN32 or a GLXContext otherwise. + static inline cl_context createContextFromType(cl_device_type deviceType, cl_int* pErrNum, void* pGLCtx = 0, void* pGLDC = 0, int preferredDeviceIndex = -1, int preferredPlatformIndex= - 1, cl_platform_id* platformId=0) + { + return b3OpenCLUtils_createContextFromType(deviceType, pErrNum, pGLCtx , pGLDC , preferredDeviceIndex, preferredPlatformIndex, platformId); + } + + static inline int getNumDevices(cl_context cxMainContext) + { + return b3OpenCLUtils_getNumDevices(cxMainContext); + } + static inline cl_device_id getDevice(cl_context cxMainContext, int nr) + { + return b3OpenCLUtils_getDevice(cxMainContext,nr); + } + + static void getDeviceInfo(cl_device_id device, b3OpenCLDeviceInfo* info); + + static inline void printDeviceInfo(cl_device_id device) + { + b3OpenCLUtils_printDeviceInfo(device); + } + + static inline cl_kernel compileCLKernelFromString( cl_context clContext,cl_device_id device, const char* kernelSource, const char* kernelName, cl_int* pErrNum=0, cl_program prog=0,const char* additionalMacros = "" ) + { + return b3OpenCLUtils_compileCLKernelFromString(clContext,device, kernelSource, kernelName, pErrNum, prog,additionalMacros); + } + + //optional + static inline cl_program compileCLProgramFromString( cl_context clContext,cl_device_id device, const char* kernelSource, cl_int* pErrNum=0,const char* additionalMacros = "" , const char* srcFileNameForCaching=0, bool disableBinaryCaching=false) + { + return b3OpenCLUtils_compileCLProgramFromString(clContext,device, kernelSource, pErrNum,additionalMacros, srcFileNameForCaching, disableBinaryCaching); + } + + //the following optional APIs provide access using specific platform information + static inline int getNumPlatforms(cl_int* pErrNum=0) + { + return b3OpenCLUtils_getNumPlatforms(pErrNum); + } + ///get the nr'th platform, where nr is in the range [0..getNumPlatforms) + static inline cl_platform_id getPlatform(int nr, cl_int* pErrNum=0) + { + return b3OpenCLUtils_getPlatform(nr,pErrNum); + } + + static void getPlatformInfo(cl_platform_id platform, b3OpenCLPlatformInfo* platformInfo); + + static inline void printPlatformInfo(cl_platform_id platform) + { + b3OpenCLUtils_printPlatformInfo(platform); + } + + static inline const char* getSdkVendorName() + { + return b3OpenCLUtils_getSdkVendorName(); + } + static inline cl_context createContextFromPlatform(cl_platform_id platform, cl_device_type deviceType, cl_int* pErrNum, void* pGLCtx = 0, void* pGLDC = 0,int preferredDeviceIndex = -1, int preferredPlatformIndex= -1) + { + return b3OpenCLUtils_createContextFromPlatform(platform, deviceType, pErrNum, pGLCtx,pGLDC,preferredDeviceIndex, preferredPlatformIndex); + } + static void setCachePath(const char* path) + { + b3OpenCLUtils_setCachePath(path); + } +}; + +#endif //__cplusplus + +#endif // B3_OPENCL_UTILS_H diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h new file mode 100644 index 000000000000..872f03950649 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h @@ -0,0 +1,18 @@ +#ifndef B3_BVH_INFO_H +#define B3_BVH_INFO_H + +#include "Bullet3Common/b3Vector3.h" + +struct b3BvhInfo +{ + b3Vector3 m_aabbMin; + b3Vector3 m_aabbMax; + b3Vector3 m_quantization; + int m_numNodes; + int m_numSubTrees; + int m_nodeOffset; + int m_subTreeOffset; + +}; + +#endif //B3_BVH_INFO_H \ No newline at end of file diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.cpp b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.cpp new file mode 100644 index 000000000000..cb30ee939b87 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.cpp @@ -0,0 +1,258 @@ + +#if 0 +/* +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 "b3ContactCache.h" +#include "Bullet3Common/b3Transform.h" + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +b3Scalar gContactBreakingThreshold = b3Scalar(0.02); + +///gContactCalcArea3Points will approximate the convex hull area using 3 points +///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower +bool gContactCalcArea3Points = true; + + + + +static inline b3Scalar calcArea4Points(const b3Vector3 &p0,const b3Vector3 &p1,const b3Vector3 &p2,const b3Vector3 &p3) +{ + // It calculates possible 3 area constructed from random 4 points and returns the biggest one. + + b3Vector3 a[3],b[3]; + a[0] = p0 - p1; + a[1] = p0 - p2; + a[2] = p0 - p3; + b[0] = p2 - p3; + b[1] = p1 - p3; + b[2] = p1 - p2; + + //todo: Following 3 cross production can be easily optimized by SIMD. + b3Vector3 tmp0 = a[0].cross(b[0]); + b3Vector3 tmp1 = a[1].cross(b[1]); + b3Vector3 tmp2 = a[2].cross(b[2]); + + return b3Max(b3Max(tmp0.length2(),tmp1.length2()),tmp2.length2()); +} +#if 0 + +//using localPointA for all points +int b3ContactCache::sortCachedPoints(const b3Vector3& 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 + b3Scalar 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 + + b3Scalar res0(b3Scalar(0.)),res1(b3Scalar(0.)),res2(b3Scalar(0.)),res3(b3Scalar(0.)); + + if (gContactCalcArea3Points) + { + if (maxPenetrationIndex != 0) + { + b3Vector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA; + b3Vector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; + b3Vector3 cross = a0.cross(b0); + res0 = cross.length2(); + } + if (maxPenetrationIndex != 1) + { + b3Vector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA; + b3Vector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; + b3Vector3 cross = a1.cross(b1); + res1 = cross.length2(); + } + + if (maxPenetrationIndex != 2) + { + b3Vector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA; + b3Vector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA; + b3Vector3 cross = a2.cross(b2); + res2 = cross.length2(); + } + + if (maxPenetrationIndex != 3) + { + b3Vector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA; + b3Vector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA; + b3Vector3 cross = a3.cross(b3); + res3 = cross.length2(); + } + } + else + { + if(maxPenetrationIndex != 0) { + res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); + } + + if(maxPenetrationIndex != 1) { + res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); + } + + if(maxPenetrationIndex != 2) { + res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA); + } + + if(maxPenetrationIndex != 3) { + res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA); + } + } + b3Vector4 maxvec(res0,res1,res2,res3); + int biggestarea = maxvec.closestAxis4(); + return biggestarea; + +} + + +int b3ContactCache::getCacheEntry(const b3Vector3& newPoint) const +{ + b3Scalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold(); + int size = getNumContacts(); + int nearestPoint = -1; + for( int i = 0; i < size; i++ ) + { + const b3Vector3 &mp = m_pointCache[i]; + + b3Vector3 diffA = mp.m_localPointA- newPoint.m_localPointA; + const b3Scalar distToManiPoint = diffA.dot(diffA); + if( distToManiPoint < shortestDist ) + { + shortestDist = distToManiPoint; + nearestPoint = i; + } + } + return nearestPoint; +} + +int b3ContactCache::addManifoldPoint(const b3Vector3& newPoint) +{ + b3Assert(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; + + //b3Assert(m_pointCache[insertIndex].m_userPersistentData==0); + m_pointCache[insertIndex] = newPoint; + return insertIndex; +} + +#endif + +bool b3ContactCache::validContactDistance(const b3Vector3& pt) +{ + return pt.w <= gContactBreakingThreshold; +} + +void b3ContactCache::removeContactPoint(struct b3Contact4Data& newContactCache,int i) +{ + int numContacts = b3Contact4Data_getNumPoints(&newContactCache); + if (i!=(numContacts-1)) + { + b3Swap(newContactCache.m_localPosA[i],newContactCache.m_localPosA[numContacts-1]); + b3Swap(newContactCache.m_localPosB[i],newContactCache.m_localPosB[numContacts-1]); + b3Swap(newContactCache.m_worldPosB[i],newContactCache.m_worldPosB[numContacts-1]); + } + b3Contact4Data_setNumPoints(&newContactCache,numContacts-1); + +} + + +void b3ContactCache::refreshContactPoints(const b3Transform& trA,const b3Transform& trB, struct b3Contact4Data& contacts) +{ + + int numContacts = b3Contact4Data_getNumPoints(&contacts); + + + int i; + /// first refresh worldspace positions and distance + for (i=numContacts-1;i>=0;i--) + { + b3Vector3 worldPosA = trA( contacts.m_localPosA[i]); + b3Vector3 worldPosB = trB( contacts.m_localPosB[i]); + contacts.m_worldPosB[i] = worldPosB; + float distance = (worldPosA - worldPosB).dot(contacts.m_worldNormalOnB); + contacts.m_worldPosB[i].w = distance; + } + + /// then + b3Scalar distance2d; + b3Vector3 projectedDifference,projectedPoint; + for (i=numContacts-1;i>=0;i--) + { + b3Vector3 worldPosA = trA( contacts.m_localPosA[i]); + b3Vector3 worldPosB = trB( contacts.m_localPosB[i]); + b3Vector3&pt = contacts.m_worldPosB[i]; + //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) + if (!validContactDistance(pt)) + { + removeContactPoint(contacts,i); + } else + { + //contact also becomes invalid when relative movement orthogonal to normal exceeds margin + projectedPoint = worldPosA - contacts.m_worldNormalOnB * contacts.m_worldPosB[i].w; + projectedDifference = contacts.m_worldPosB[i] - projectedPoint; + distance2d = projectedDifference.dot(projectedDifference); + if (distance2d > gContactBreakingThreshold*gContactBreakingThreshold ) + { + removeContactPoint(contacts,i); + } else + { + ////contact point processed callback + //if (gContactProcessedCallback) + // (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1); + } + } + } + + +} + + + + + +#endif diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h new file mode 100644 index 000000000000..d6c9b0a07e4e --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h @@ -0,0 +1,80 @@ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 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. +*/ + +#ifndef B3_CONTACT_CACHE_H +#define B3_CONTACT_CACHE_H + + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Transform.h" +#include "Bullet3Common/b3AlignedAllocator.h" + + +///maximum contact breaking and merging threshold +extern b3Scalar gContactBreakingThreshold; + + + +#define MANIFOLD_CACHE_SIZE 4 + +///b3ContactCache is a contact point cache, it stays persistent as long as objects are overlapping in the broadphase. +///Those contact points are created by the collision narrow phase. +///The cache can be empty, or hold 1,2,3 or 4 points. Some collision algorithms (GJK) might only add one point at a time. +///updates/refreshes old contact points, and throw them away if necessary (distance becomes too large) +///reduces the cache to 4 points, when more then 4 points are added, using following rules: +///the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points +///note that some pairs of objects might have more then one contact manifold. +B3_ATTRIBUTE_ALIGNED16( class) b3ContactCache +{ + + + + + /// sort cached points so most isolated points come first + int sortCachedPoints(const b3Vector3& pt); + + + +public: + + B3_DECLARE_ALIGNED_ALLOCATOR(); + + + + int addManifoldPoint( const b3Vector3& newPoint); + + /*void replaceContactPoint(const b3Vector3& newPoint,int insertIndex) + { + b3Assert(validContactDistance(newPoint)); + m_pointCache[insertIndex] = newPoint; + } + */ + + + + static bool validContactDistance(const b3Vector3& pt); + + /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin + static void refreshContactPoints( const b3Transform& trA,const b3Transform& trB, struct b3Contact4Data& newContactCache); + + static void removeContactPoint(struct b3Contact4Data& newContactCache,int i); + + +}; + + + +#endif //B3_CONTACT_CACHE_H diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp new file mode 100644 index 000000000000..fb435aa7fddc --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp @@ -0,0 +1,4733 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011 Advanced Micro Devices, Inc. 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. +*/ + +bool findSeparatingAxisOnGpu = true; +bool splitSearchSepAxisConcave = false; +bool splitSearchSepAxisConvex = true; +bool useMprGpu = true;//use mpr for edge-edge (+contact point) or sat. Needs testing on main OpenCL platforms, before enabling... +bool bvhTraversalKernelGPU = true; +bool findConcaveSeparatingAxisKernelGPU = true; +bool clipConcaveFacesAndFindContactsCPU = false;//false;//true; +bool clipConvexFacesAndFindContactsCPU = false;//false;//true; +bool reduceConcaveContactsOnGPU = true;//false; +bool reduceConvexContactsOnGPU = true;//false; +bool findConvexClippingFacesGPU = true; +bool useGjk = false;///option for CPU/host testing, when findSeparatingAxisOnGpu = false +bool useGjkContacts = false;//////option for CPU/host testing when findSeparatingAxisOnGpu = false + + +static int myframecount=0;///for testing + +///This file was written by Erwin Coumans +///Separating axis rest based on work from Pierre Terdiman, see +///And contact clipping based on work from Simon Hobbs + +//#define B3_DEBUG_SAT_FACE + +//#define CHECK_ON_HOST + +#ifdef CHECK_ON_HOST +//#define PERSISTENT_CONTACTS_HOST +#endif + +int b3g_actualSATPairTests=0; + +#include "b3ConvexHullContact.h" +#include //memcpy +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h" + +#include "Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.h" +#include "Bullet3Geometry/b3AabbUtil.h" + +typedef b3AlignedObjectArray b3VertexArray; + + +#include //for FLT_MAX +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" +//#include "AdlQuaternion.h" + +#include "kernels/satKernels.h" +#include "kernels/mprKernels.h" + +#include "kernels/satConcaveKernels.h" + +#include "kernels/satClipHullContacts.h" +#include "kernels/bvhTraversal.h" +#include "kernels/primitiveContacts.h" + + +#include "Bullet3Geometry/b3AabbUtil.h" + +#define BT_NARROWPHASE_SAT_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl" +#define BT_NARROWPHASE_SAT_CONCAVE_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/satConcave.cl" + +#define BT_NARROWPHASE_MPR_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/mpr.cl" + + +#define BT_NARROWPHASE_CLIPHULL_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl" +#define BT_NARROWPHASE_BVH_TRAVERSAL_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl" +#define BT_NARROWPHASE_PRIMITIVE_CONTACT_PATH "src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.cl" + + +#ifndef __global +#define __global +#endif + +#ifndef __kernel +#define __kernel +#endif + + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h" + + + +#define dot3F4 b3Dot + +GpuSatCollision::GpuSatCollision(cl_context ctx,cl_device_id device, cl_command_queue q ) +:m_context(ctx), +m_device(device), +m_queue(q), + +m_findSeparatingAxisKernel(0), +m_findSeparatingAxisVertexFaceKernel(0), +m_findSeparatingAxisEdgeEdgeKernel(0), +m_unitSphereDirections(m_context,m_queue), + +m_totalContactsOut(m_context, m_queue), +m_sepNormals(m_context, m_queue), +m_dmins(m_context,m_queue), + +m_hasSeparatingNormals(m_context, m_queue), +m_concaveSepNormals(m_context, m_queue), +m_concaveHasSeparatingNormals(m_context,m_queue), +m_numConcavePairsOut(m_context, m_queue), + + +m_gpuCompoundPairs(m_context, m_queue), + + +m_gpuCompoundSepNormals(m_context, m_queue), +m_gpuHasCompoundSepNormals(m_context, m_queue), + +m_numCompoundPairsOut(m_context, m_queue) +{ + m_totalContactsOut.push_back(0); + + cl_int errNum=0; + + if (1) + { + const char* mprSrc = mprKernelsCL; + + const char* srcConcave = satConcaveKernelsCL; + char flags[1024]={0}; +//#ifdef CL_PLATFORM_INTEL +// sprintf(flags,"-g -s \"%s\"","C:/develop/bullet3_experiments2/opencl/gpu_narrowphase/kernels/sat.cl"); +//#endif + m_mprPenetrationKernel = 0; + m_findSeparatingAxisUnitSphereKernel = 0; + + if (useMprGpu) + { + cl_program mprProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,mprSrc,&errNum,flags,BT_NARROWPHASE_MPR_PATH); + b3Assert(errNum==CL_SUCCESS); + + m_mprPenetrationKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,mprSrc, "mprPenetrationKernel",&errNum,mprProg ); + b3Assert(m_mprPenetrationKernel); + b3Assert(errNum==CL_SUCCESS); + + m_findSeparatingAxisUnitSphereKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,mprSrc, "findSeparatingAxisUnitSphereKernel",&errNum,mprProg ); + b3Assert(m_findSeparatingAxisUnitSphereKernel); + b3Assert(errNum==CL_SUCCESS); + + + int numDirections = sizeof(unitSphere162)/sizeof(b3Vector3); + m_unitSphereDirections.resize(numDirections); + m_unitSphereDirections.copyFromHostPointer(unitSphere162,numDirections,0,true); + + + } + + + cl_program satProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,satKernelsCL,&errNum,flags,BT_NARROWPHASE_SAT_PATH); + b3Assert(errNum==CL_SUCCESS); + + cl_program satConcaveProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,srcConcave,&errNum,flags,BT_NARROWPHASE_SAT_CONCAVE_PATH); + b3Assert(errNum==CL_SUCCESS); + + m_findSeparatingAxisKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,satKernelsCL, "findSeparatingAxisKernel",&errNum,satProg ); + b3Assert(m_findSeparatingAxisKernel); + b3Assert(errNum==CL_SUCCESS); + + + m_findSeparatingAxisVertexFaceKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,satKernelsCL, "findSeparatingAxisVertexFaceKernel",&errNum,satProg ); + b3Assert(m_findSeparatingAxisVertexFaceKernel); + + m_findSeparatingAxisEdgeEdgeKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,satKernelsCL, "findSeparatingAxisEdgeEdgeKernel",&errNum,satProg ); + b3Assert(m_findSeparatingAxisVertexFaceKernel); + + + m_findConcaveSeparatingAxisKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,satKernelsCL, "findConcaveSeparatingAxisKernel",&errNum,satProg ); + b3Assert(m_findConcaveSeparatingAxisKernel); + b3Assert(errNum==CL_SUCCESS); + + m_findConcaveSeparatingAxisVertexFaceKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,srcConcave, "findConcaveSeparatingAxisVertexFaceKernel",&errNum,satConcaveProg ); + b3Assert(m_findConcaveSeparatingAxisVertexFaceKernel); + b3Assert(errNum==CL_SUCCESS); + + m_findConcaveSeparatingAxisEdgeEdgeKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,srcConcave, "findConcaveSeparatingAxisEdgeEdgeKernel",&errNum,satConcaveProg ); + b3Assert(m_findConcaveSeparatingAxisEdgeEdgeKernel); + b3Assert(errNum==CL_SUCCESS); + + + + + m_findCompoundPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,satKernelsCL, "findCompoundPairsKernel",&errNum,satProg ); + b3Assert(m_findCompoundPairsKernel); + b3Assert(errNum==CL_SUCCESS); + m_processCompoundPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,satKernelsCL, "processCompoundPairsKernel",&errNum,satProg ); + b3Assert(m_processCompoundPairsKernel); + b3Assert(errNum==CL_SUCCESS); + } + + if (1) + { + const char* srcClip = satClipKernelsCL; + + char flags[1024]={0}; +//#ifdef CL_PLATFORM_INTEL +// sprintf(flags,"-g -s \"%s\"","C:/develop/bullet3_experiments2/opencl/gpu_narrowphase/kernels/satClipHullContacts.cl"); +//#endif + + cl_program satClipContactsProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,srcClip,&errNum,flags,BT_NARROWPHASE_CLIPHULL_PATH); + b3Assert(errNum==CL_SUCCESS); + + m_clipHullHullKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,srcClip, "clipHullHullKernel",&errNum,satClipContactsProg); + b3Assert(errNum==CL_SUCCESS); + + m_clipCompoundsHullHullKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,srcClip, "clipCompoundsHullHullKernel",&errNum,satClipContactsProg); + b3Assert(errNum==CL_SUCCESS); + + + m_findClippingFacesKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,srcClip, "findClippingFacesKernel",&errNum,satClipContactsProg); + b3Assert(errNum==CL_SUCCESS); + + m_clipFacesAndFindContacts = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,srcClip, "clipFacesAndFindContactsKernel",&errNum,satClipContactsProg); + b3Assert(errNum==CL_SUCCESS); + + m_clipHullHullConcaveConvexKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,srcClip, "clipHullHullConcaveConvexKernel",&errNum,satClipContactsProg); + b3Assert(errNum==CL_SUCCESS); + +// m_extractManifoldAndAddContactKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,srcClip, "extractManifoldAndAddContactKernel",&errNum,satClipContactsProg); + // b3Assert(errNum==CL_SUCCESS); + + m_newContactReductionKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,srcClip, + "newContactReductionKernel",&errNum,satClipContactsProg); + b3Assert(errNum==CL_SUCCESS); + } + else + { + m_clipHullHullKernel=0; + m_clipCompoundsHullHullKernel = 0; + m_findClippingFacesKernel = 0; + m_newContactReductionKernel=0; + m_clipFacesAndFindContacts = 0; + m_clipHullHullConcaveConvexKernel = 0; +// m_extractManifoldAndAddContactKernel = 0; + } + + if (1) + { + const char* srcBvh = bvhTraversalKernelCL; + cl_program bvhTraversalProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,srcBvh,&errNum,"",BT_NARROWPHASE_BVH_TRAVERSAL_PATH); + b3Assert(errNum==CL_SUCCESS); + + m_bvhTraversalKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,srcBvh, "bvhTraversalKernel",&errNum,bvhTraversalProg,""); + b3Assert(errNum==CL_SUCCESS); + + } + + { + const char* primitiveContactsSrc = primitiveContactsKernelsCL; + cl_program primitiveContactsProg = b3OpenCLUtils::compileCLProgramFromString(m_context,m_device,primitiveContactsSrc,&errNum,"",BT_NARROWPHASE_PRIMITIVE_CONTACT_PATH); + b3Assert(errNum==CL_SUCCESS); + + m_primitiveContactsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "primitiveContactsKernel",&errNum,primitiveContactsProg,""); + b3Assert(errNum==CL_SUCCESS); + + m_findConcaveSphereContactsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "findConcaveSphereContactsKernel",&errNum,primitiveContactsProg ); + b3Assert(errNum==CL_SUCCESS); + b3Assert(m_findConcaveSphereContactsKernel); + + m_processCompoundPairsPrimitivesKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device,primitiveContactsSrc, "processCompoundPairsPrimitivesKernel",&errNum,primitiveContactsProg,""); + b3Assert(errNum==CL_SUCCESS); + b3Assert(m_processCompoundPairsPrimitivesKernel); + + } + + +} + +GpuSatCollision::~GpuSatCollision() +{ + + if (m_findSeparatingAxisVertexFaceKernel) + clReleaseKernel(m_findSeparatingAxisVertexFaceKernel); + + if (m_findSeparatingAxisEdgeEdgeKernel) + clReleaseKernel(m_findSeparatingAxisEdgeEdgeKernel); + + if (m_findSeparatingAxisUnitSphereKernel) + clReleaseKernel(m_findSeparatingAxisUnitSphereKernel); + + if (m_mprPenetrationKernel) + clReleaseKernel(m_mprPenetrationKernel); + + + if (m_findSeparatingAxisKernel) + clReleaseKernel(m_findSeparatingAxisKernel); + + if (m_findConcaveSeparatingAxisVertexFaceKernel) + clReleaseKernel(m_findConcaveSeparatingAxisVertexFaceKernel); + + + if (m_findConcaveSeparatingAxisEdgeEdgeKernel) + clReleaseKernel(m_findConcaveSeparatingAxisEdgeEdgeKernel); + + if (m_findConcaveSeparatingAxisKernel) + clReleaseKernel(m_findConcaveSeparatingAxisKernel); + + if (m_findCompoundPairsKernel) + clReleaseKernel(m_findCompoundPairsKernel); + + if (m_processCompoundPairsKernel) + clReleaseKernel(m_processCompoundPairsKernel); + + if (m_findClippingFacesKernel) + clReleaseKernel(m_findClippingFacesKernel); + + if (m_clipFacesAndFindContacts) + clReleaseKernel(m_clipFacesAndFindContacts); + if (m_newContactReductionKernel) + clReleaseKernel(m_newContactReductionKernel); + if (m_primitiveContactsKernel) + clReleaseKernel(m_primitiveContactsKernel); + + if (m_findConcaveSphereContactsKernel) + clReleaseKernel(m_findConcaveSphereContactsKernel); + + if (m_processCompoundPairsPrimitivesKernel) + clReleaseKernel(m_processCompoundPairsPrimitivesKernel); + + if (m_clipHullHullKernel) + clReleaseKernel(m_clipHullHullKernel); + if (m_clipCompoundsHullHullKernel) + clReleaseKernel(m_clipCompoundsHullHullKernel); + + if (m_clipHullHullConcaveConvexKernel) + clReleaseKernel(m_clipHullHullConcaveConvexKernel); +// if (m_extractManifoldAndAddContactKernel) + // clReleaseKernel(m_extractManifoldAndAddContactKernel); + + if (m_bvhTraversalKernel) + clReleaseKernel(m_bvhTraversalKernel); + +} + +struct MyTriangleCallback : public b3NodeOverlapCallback +{ + int m_bodyIndexA; + int m_bodyIndexB; + + virtual void processNode(int subPart, int triangleIndex) + { + printf("bodyIndexA %d, bodyIndexB %d\n",m_bodyIndexA,m_bodyIndexB); + printf("triangleIndex %d\n", triangleIndex); + } +}; + + +#define float4 b3Vector3 +#define make_float4(x,y,z,w) b3MakeVector3(x,y,z,w) + +float signedDistanceFromPointToPlane(const float4& point, const float4& planeEqn, float4* closestPointOnFace) +{ + float4 n = planeEqn; + n[3] = 0.f; + float dist = dot3F4(n, point) + planeEqn[3]; + *closestPointOnFace = point - dist * n; + return dist; +} + + + +#define cross3(a,b) (a.cross(b)) +b3Vector3 transform(const b3Vector3* v, const b3Vector3* pos, const b3Quaternion* orn) +{ + b3Transform tr; + tr.setIdentity(); + tr.setOrigin(*pos); + tr.setRotation(*orn); + b3Vector3 res = tr(*v); + return res; +} + + +inline bool IsPointInPolygon(const float4& p, + const b3GpuFace* face, + const float4* baseVertex, + const int* convexIndices, + float4* out) +{ + float4 a; + float4 b; + float4 ab; + float4 ap; + float4 v; + + float4 plane = b3MakeVector3(face->m_plane.x,face->m_plane.y,face->m_plane.z,0.f); + + if (face->m_numIndices<2) + return false; + + + float4 v0 = baseVertex[convexIndices[face->m_indexOffset + face->m_numIndices-1]]; + b = v0; + + for(unsigned i=0; i != face->m_numIndices; ++i) + { + a = b; + float4 vi = baseVertex[convexIndices[face->m_indexOffset + i]]; + b = vi; + ab = b-a; + ap = p-a; + v = cross3(ab,plane); + + if (b3Dot(ap, v) > 0.f) + { + float ab_m2 = b3Dot(ab, ab); + float rt = ab_m2 != 0.f ? b3Dot(ab, ap) / ab_m2 : 0.f; + if (rt <= 0.f) + { + *out = a; + } + else if (rt >= 1.f) + { + *out = b; + } + else + { + float s = 1.f - rt; + out[0].x = s * a.x + rt * b.x; + out[0].y = s * a.y + rt * b.y; + out[0].z = s * a.z + rt * b.z; + } + return false; + } + } + return true; +} + +#define normalize3(a) (a.normalize()) + + +int extractManifoldSequentialGlobal( const float4* p, int nPoints, const float4& nearNormal, b3Int4* contactIdx) +{ + if( nPoints == 0 ) + return 0; + + if (nPoints <=4) + return nPoints; + + + if (nPoints >64) + nPoints = 64; + + float4 center = b3MakeVector3(0,0,0,0); + { + + for (int i=0;i& vertices, b3Scalar& min, b3Scalar& max) +{ + min = FLT_MAX; + max = -FLT_MAX; + int numVerts = hull.m_numVertices; + + const float4 localDir = b3QuatRotate(orn.inverse(),dir); + + b3Scalar offset = dot3F4(pos,dir); + + for(int i=0;i max) max = dp; + } + if(min>max) + { + b3Scalar tmp = min; + min = max; + max = tmp; + } + min += offset; + max += offset; +} + + +static bool TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const float4& posA,const b3Quaternion& ornA, + const float4& posB,const b3Quaternion& ornB, + const float4& sep_axis, const b3AlignedObjectArray& verticesA,const b3AlignedObjectArray& verticesB,b3Scalar& depth) +{ + b3Scalar Min0,Max0; + b3Scalar Min1,Max1; + project(hullA,posA,ornA,sep_axis,verticesA, Min0, Max0); + project(hullB,posB,ornB, sep_axis,verticesB, Min1, Max1); + + if(Max0=0.0f); + b3Scalar d1 = Max1 - Min0; + assert(d1>=0.0f); + depth = d01e-6 || fabsf(v.y)>1e-6 || fabsf(v.z)>1e-6) return false; + return true; +} + + +static bool findSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const float4& posA1, + const b3Quaternion& ornA, + const float4& posB1, + const b3Quaternion& ornB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& uniqueEdgesA, + const b3AlignedObjectArray& facesA, + const b3AlignedObjectArray& indicesA, + const b3AlignedObjectArray& verticesB, + const b3AlignedObjectArray& uniqueEdgesB, + const b3AlignedObjectArray& facesB, + const b3AlignedObjectArray& indicesB, + + b3Vector3& sep) +{ + B3_PROFILE("findSeparatingAxis"); + + b3g_actualSATPairTests++; + float4 posA = posA1; + posA.w = 0.f; + float4 posB = posB1; + posB.w = 0.f; +//#ifdef TEST_INTERNAL_OBJECTS + float4 c0local = (float4&)hullA.m_localCenter; + float4 c0 = transform(&c0local, &posA, &ornA); + float4 c1local = (float4&)hullB.m_localCenter; + float4 c1 = transform(&c1local,&posB,&ornB); + const float4 deltaC2 = c0 - c1; +//#endif + + b3Scalar dmin = FLT_MAX; + int curPlaneTests=0; + + int numFacesA = hullA.m_numFaces; + // Test normals from hullA + for(int i=0;i0.0f) + sep = -sep; + + return true; +} + + +bool findSeparatingAxisEdgeEdge( __global const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, + const b3Float4& posA1, + const b3Quat& ornA, + const b3Float4& posB1, + const b3Quat& ornB, + const b3Float4& DeltaC2, + __global const b3AlignedObjectArray& vertices, + __global const b3AlignedObjectArray& uniqueEdges, + __global const b3AlignedObjectArray& faces, + __global const b3AlignedObjectArray& indices, + float4* sep, + float* dmin) +{ +// int i = get_global_id(0); + + float4 posA = posA1; + posA.w = 0.f; + float4 posB = posB1; + posB.w = 0.f; + + //int curPlaneTests=0; + + int curEdgeEdge = 0; + // Test edges + for(int e0=0;e0m_numUniqueEdges;e0++) + { + const float4 edge0 = uniqueEdges[hullA->m_uniqueEdgesOffset+e0]; + float4 edge0World = b3QuatRotate(ornA,edge0); + + for(int e1=0;e1m_numUniqueEdges;e1++) + { + const float4 edge1 = uniqueEdges[hullB->m_uniqueEdgesOffset+e1]; + float4 edge1World = b3QuatRotate(ornB,edge1); + + + float4 crossje = cross3(edge0World,edge1World); + + curEdgeEdge++; + if(!IsAlmostZero(crossje)) + { + crossje = normalize3(crossje); + if (dot3F4(DeltaC2,crossje)<0) + crossje*=-1.f; + + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + project(*hullA,posA,ornA,crossje,vertices, Min0, Max0); + project(*hullB,posB,ornB,crossje,vertices, Min1, Max1); + + if(Max00.0f) + { + *sep = -(*sep); + } + return true; +} + + +__inline float4 lerp3(const float4& a,const float4& b, float t) +{ + return b3MakeVector3( a.x + (b.x - a.x) * t, + a.y + (b.y - a.y) * t, + a.z + (b.z - a.z) * t, + 0.f); +} + + +// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut +int clipFace(const float4* pVtxIn, int numVertsIn, float4& planeNormalWS,float planeEqWS, float4* ppVtxOut) +{ + + int ve; + float ds, de; + int numVertsOut = 0; + if (numVertsIn < 2) + return 0; + + float4 firstVertex=pVtxIn[numVertsIn-1]; + float4 endVertex = pVtxIn[0]; + + ds = dot3F4(planeNormalWS,firstVertex)+planeEqWS; + + for (ve = 0; ve < numVertsIn; ve++) + { + endVertex=pVtxIn[ve]; + + de = dot3F4(planeNormalWS,endVertex)+planeEqWS; + + if (ds<0) + { + if (de<0) + { + // Start < 0, end < 0, so output endVertex + ppVtxOut[numVertsOut++] = endVertex; + } + else + { + // Start < 0, end >= 0, so output intersection + ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + } + } + else + { + if (de<0) + { + // Start >= 0, end < 0 so output intersection and end + ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + ppVtxOut[numVertsOut++] = endVertex; + } + } + firstVertex = endVertex; + ds = de; + } + return numVertsOut; +} + + +int clipFaceAgainstHull(const float4& separatingNormal, const b3ConvexPolyhedronData* hullA, + const float4& posA, const b3Quaternion& ornA, float4* worldVertsB1, int numWorldVertsB1, + float4* worldVertsB2, int capacityWorldVertsB2, + const float minDist, float maxDist, + const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& facesA, const b3AlignedObjectArray& indicesA, + //const float4* verticesB, const b3GpuFace* facesB, const int* indicesB, + float4* contactsOut, + int contactCapacity) +{ + int numContactsOut = 0; + + float4* pVtxIn = worldVertsB1; + float4* pVtxOut = worldVertsB2; + + int numVertsIn = numWorldVertsB1; + int numVertsOut = 0; + + int closestFaceA=-1; + { + float dmin = FLT_MAX; + for(int face=0;facem_numFaces;face++) + { + const float4 Normal = b3MakeVector3( + facesA[hullA->m_faceOffset+face].m_plane.x, + facesA[hullA->m_faceOffset+face].m_plane.y, + facesA[hullA->m_faceOffset+face].m_plane.z,0.f); + const float4 faceANormalWS = b3QuatRotate(ornA,Normal); + + float d = dot3F4(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + } + } + } + if (closestFaceA<0) + return numContactsOut; + + b3GpuFace polyA = facesA[hullA->m_faceOffset+closestFaceA]; + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face +// int numContacts = numWorldVertsB1; + int numVerticesA = polyA.m_numIndices; + for(int e0=0;e0m_vertexOffset+indicesA[polyA.m_indexOffset+e0]]; + const float4 b = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+((e0+1)%numVerticesA)]]; + const float4 edge0 = a - b; + const float4 WorldEdge0 = b3QuatRotate(ornA,edge0); + float4 planeNormalA = make_float4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + float4 worldPlaneAnormal1 = b3QuatRotate(ornA,planeNormalA); + + float4 planeNormalWS1 = -cross3(WorldEdge0,worldPlaneAnormal1); + float4 worldA1 = transform(&a,&posA,&ornA); + float planeEqWS1 = -dot3F4(worldA1,planeNormalWS1); + + float4 planeNormalWS = planeNormalWS1; + float planeEqWS=planeEqWS1; + + //clip face + //clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS); + numVertsOut = clipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut); + + //btSwap(pVtxIn,pVtxOut); + float4* tmp = pVtxOut; + pVtxOut = pVtxIn; + pVtxIn = tmp; + numVertsIn = numVertsOut; + numVertsOut = 0; + } + + + // only keep points that are behind the witness face + { + float4 localPlaneNormal = make_float4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + float localPlaneEq = polyA.m_plane.w; + float4 planeNormalWS = b3QuatRotate(ornA,localPlaneNormal); + float planeEqWS=localPlaneEq-dot3F4(planeNormalWS,posA); + for (int i=0;i& verticesA, const b3AlignedObjectArray& facesA, const b3AlignedObjectArray& indicesA, + const b3AlignedObjectArray& verticesB, const b3AlignedObjectArray& facesB, const b3AlignedObjectArray& indicesB, + + float4* contactsOut, + int contactCapacity) +{ + int numContactsOut = 0; + int numWorldVertsB1= 0; + + B3_PROFILE("clipHullAgainstHull"); + +// float curMaxDist=maxDist; + int closestFaceB=-1; + float dmax = -FLT_MAX; + + { + //B3_PROFILE("closestFaceB"); + if (hullB.m_numFaces!=1) + { + //printf("wtf\n"); + } + static bool once = true; + //printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z); + + for(int face=0;facem_numIndices;i++) + { + float4 vert = verticesB[hullB.m_vertexOffset+indicesB[faceB->m_indexOffset+i]]; + printf("vert[%d] = %f,%f,%f\n",i,vert.x,vert.y,vert.z); + } + } +#endif //BT_DEBUG_SAT_FACE + //if (facesB[hullB.m_faceOffset+face].m_numIndices>2) + { + const float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset+face].m_plane.x, + facesB[hullB.m_faceOffset+face].m_plane.y, facesB[hullB.m_faceOffset+face].m_plane.z,0.f); + const float4 WorldNormal = b3QuatRotate(ornB, Normal); +#ifdef BT_DEBUG_SAT_FACE + if (once) + printf("faceNormal = %f,%f,%f\n",Normal.x,Normal.y,Normal.z); +#endif + float d = dot3F4(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + once = false; + } + + + b3Assert(closestFaceB>=0); + { + //B3_PROFILE("worldVertsB1"); + const b3GpuFace& polyB = facesB[hullB.m_faceOffset+closestFaceB]; + const int numVertices = polyB.m_numIndices; + for(int e0=0;e0=0) + { + //B3_PROFILE("clipFaceAgainstHull"); + numContactsOut = clipFaceAgainstHull((float4&)separatingNormal, &hullA, + posA,ornA, + worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist, + verticesA, facesA, indicesA, + contactsOut,contactCapacity); + } + + return numContactsOut; +} + + + + + + +#define PARALLEL_SUM(v, n) for(int j=1; j v[i+offset].y)? v[i]: v[i+offset]; } +#define REDUCE_MIN(v, n) {int i=0;\ +for(int offset=0; offset64) + nPoints = 64; + + float4 center = make_float4(0,0,0,0); + { + + for (int i=0;i* bodyBuf, + b3AlignedObjectArray* globalContactOut, + int& nContacts, + + const b3AlignedObjectArray& hostConvexDataA, + const b3AlignedObjectArray& hostConvexDataB, + + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& uniqueEdgesA, + const b3AlignedObjectArray& facesA, + const b3AlignedObjectArray& indicesA, + + const b3AlignedObjectArray& verticesB, + const b3AlignedObjectArray& uniqueEdgesB, + const b3AlignedObjectArray& facesB, + const b3AlignedObjectArray& indicesB, + + const b3AlignedObjectArray& hostCollidablesA, + const b3AlignedObjectArray& hostCollidablesB, + const b3Vector3& sepNormalWorldSpace, + int maxContactCapacity ) +{ + int contactIndex = -1; + b3ConvexPolyhedronData hullA, hullB; + + b3Collidable colA = hostCollidablesA[collidableIndexA]; + hullA = hostConvexDataA[colA.m_shapeIndex]; + //printf("numvertsA = %d\n",hullA.m_numVertices); + + + b3Collidable colB = hostCollidablesB[collidableIndexB]; + hullB = hostConvexDataB[colB.m_shapeIndex]; + //printf("numvertsB = %d\n",hullB.m_numVertices); + + + float4 contactsOut[MAX_VERTS]; + int localContactCapacity = MAX_VERTS; + +#ifdef _WIN32 + b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x)); + b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x)); +#endif + + + { + + float4 worldVertsB1[MAX_VERTS]; + float4 worldVertsB2[MAX_VERTS]; + int capacityWorldVerts = MAX_VERTS; + + float4 hostNormal = make_float4(sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z,0.f); + int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex; + int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex; + + b3Scalar minDist = -1; + b3Scalar maxDist = 0.; + + + + b3Transform trA,trB; + { + //B3_PROFILE("transform computation"); + //trA.setIdentity(); + trA.setOrigin(b3MakeVector3(posA.x,posA.y,posA.z)); + trA.setRotation(b3Quaternion(ornA.x,ornA.y,ornA.z,ornA.w)); + + //trB.setIdentity(); + trB.setOrigin(b3MakeVector3(posB.x,posB.y,posB.z)); + trB.setRotation(b3Quaternion(ornB.x,ornB.y,ornB.z,ornB.w)); + } + + b3Quaternion trAorn = trA.getRotation(); + b3Quaternion trBorn = trB.getRotation(); + + int numContactsOut = clipHullAgainstHull(hostNormal, + hostConvexDataA.at(shapeA), + hostConvexDataB.at(shapeB), + (float4&)trA.getOrigin(), (b3Quaternion&)trAorn, + (float4&)trB.getOrigin(), (b3Quaternion&)trBorn, + worldVertsB1,worldVertsB2,capacityWorldVerts, + minDist, maxDist, + verticesA, facesA,indicesA, + verticesB, facesB,indicesB, + + contactsOut,localContactCapacity); + + if (numContactsOut>0) + { + B3_PROFILE("overlap"); + + float4 normalOnSurfaceB = (float4&)hostNormal; + + b3Int4 contactIdx; + contactIdx.x = 0; + contactIdx.y = 1; + contactIdx.z = 2; + contactIdx.w = 3; + + int numPoints = 0; + + { + // B3_PROFILE("extractManifold"); + numPoints = extractManifold(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx); + } + + b3Assert(numPoints); + + if (nContactsexpand(); + b3Contact4& contact = globalContactOut->at(nContacts); + contact.m_batchIdx = 0;//i; + contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA; + contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB; + + contact.m_frictionCoeffCmp = 45874; + contact.m_restituitionCoeffCmp = 0; + + // float distance = 0.f; + for (int p=0;pm_numVertices;i++) + { + b3Vector3 vtx = convexVertices[hullB->m_vertexOffset+i]; + float curDot = vtx.dot(planeNormalInConvex); + + + if (curDot>maxDot) + { + hitVertex=i; + maxDot=curDot; + hitVtx = vtx; + //make sure the deepest points is always included + if (numPoints==MAX_PLANE_CONVEX_POINTS) + numPoints--; + } + + if (numPoints4) + { + numReducedPoints = extractManifoldSequentialGlobal( contactPoints, numPoints, planeNormalInConvex, &contactIdx); + } + int dstIdx; +// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx ); + + if (numReducedPoints>0) + { + if (nGlobalContactsOut < maxContactCapacity) + { + dstIdx=nGlobalContactsOut; + nGlobalContactsOut++; + + b3Contact4* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = -planeNormalWorld; + c->setFrictionCoeff(0.7); + c->setRestituitionCoeff(0.f); + + c->m_batchIdx = pairIndex; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB; + for (int i=0;im_worldPosB[i] = pOnB1; + } + c->m_worldNormalOnB.w = (b3Scalar)numReducedPoints; + }//if (dstIdx < numPairs) + } + + + +// printf("computeContactPlaneConvex\n"); +} + + + +B3_FORCE_INLINE b3Vector3 MyUnQuantize(const unsigned short* vecIn, const b3Vector3& quantization, const b3Vector3& bvhAabbMin) + { + b3Vector3 vecOut; + vecOut.setValue( + (b3Scalar)(vecIn[0]) / (quantization.x), + (b3Scalar)(vecIn[1]) / (quantization.y), + (b3Scalar)(vecIn[2]) / (quantization.z)); + vecOut += bvhAabbMin; + return vecOut; + } + +void traverseTreeTree() +{ + +} + +#include "Bullet3Common/shared/b3Mat3x3.h" + +int numAabbChecks = 0; +int maxNumAabbChecks = 0; +int maxDepth = 0; + +// work-in-progress +__kernel void findCompoundPairsKernel( + int pairIndex, + int bodyIndexA, + int bodyIndexB, + int collidableIndexA, + int collidableIndexB, + __global const b3RigidBodyData* rigidBodies, + __global const b3Collidable* collidables, + __global const b3ConvexPolyhedronData* convexShapes, + __global const b3AlignedObjectArray& vertices, + __global const b3AlignedObjectArray& aabbsWorldSpace, + __global const b3AlignedObjectArray& aabbsLocalSpace, + __global const b3GpuChildShape* gpuChildShapes, + __global b3Int4* gpuCompoundPairsOut, + __global int* numCompoundPairsOut, + int maxNumCompoundPairsCapacity, + b3AlignedObjectArray& treeNodesCPU, + b3AlignedObjectArray& subTreesCPU, + b3AlignedObjectArray& bvhInfoCPU + ) +{ + numAabbChecks=0; + maxNumAabbChecks=0; +// int i = pairIndex; + { + + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + + //once the broadphase avoids static-static pairs, we can remove this test + if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0)) + { + return; + } + + if ((collidables[collidableIndexA].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) &&(collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)) + { + int bvhA = collidables[collidableIndexA].m_compoundBvhIndex; + int bvhB = collidables[collidableIndexB].m_compoundBvhIndex; + int numSubTreesA = bvhInfoCPU[bvhA].m_numSubTrees; + int subTreesOffsetA = bvhInfoCPU[bvhA].m_subTreeOffset; + int subTreesOffsetB = bvhInfoCPU[bvhB].m_subTreeOffset; + + + int numSubTreesB = bvhInfoCPU[bvhB].m_numSubTrees; + + float4 posA = rigidBodies[bodyIndexA].m_pos; + b3Quat ornA = rigidBodies[bodyIndexA].m_quat; + + b3Transform transA; + transA.setIdentity(); + transA.setOrigin(posA); + transA.setRotation(ornA); + + b3Quat ornB = rigidBodies[bodyIndexB].m_quat; + float4 posB = rigidBodies[bodyIndexB].m_pos; + + b3Transform transB; + transB.setIdentity(); + transB.setOrigin(posB); + transB.setRotation(ornB); + + + + for (int p=0;p nodeStack; + b3Int2 node0; + node0.x = startNodeIndexA; + node0.y = startNodeIndexB; + + int maxStackDepth = 1024; + nodeStack.resize(maxStackDepth); + int depth=0; + nodeStack[depth++]=node0; + + do + { + if (depth > maxDepth) + { + maxDepth=depth; + printf("maxDepth=%d\n",maxDepth); + } + b3Int2 node = nodeStack[--depth]; + + b3Vector3 aMinLocal = MyUnQuantize(treeNodesCPU[node.x].m_quantizedAabbMin,bvhInfoCPU[bvhA].m_quantization,bvhInfoCPU[bvhA].m_aabbMin); + b3Vector3 aMaxLocal = MyUnQuantize(treeNodesCPU[node.x].m_quantizedAabbMax,bvhInfoCPU[bvhA].m_quantization,bvhInfoCPU[bvhA].m_aabbMin); + + b3Vector3 bMinLocal = MyUnQuantize(treeNodesCPU[node.y].m_quantizedAabbMin,bvhInfoCPU[bvhB].m_quantization,bvhInfoCPU[bvhB].m_aabbMin); + b3Vector3 bMaxLocal = MyUnQuantize(treeNodesCPU[node.y].m_quantizedAabbMax,bvhInfoCPU[bvhB].m_quantization,bvhInfoCPU[bvhB].m_aabbMin); + + float margin=0.f; + b3Vector3 aabbAMinOut,aabbAMaxOut; + b3TransformAabb2(aMinLocal,aMaxLocal, margin,transA.getOrigin(),transA.getRotation(),&aabbAMinOut,&aabbAMaxOut); + + b3Vector3 aabbBMinOut,aabbBMaxOut; + b3TransformAabb2(bMinLocal,bMaxLocal, margin,transB.getOrigin(),transB.getRotation(),&aabbBMinOut,&aabbBMaxOut); + + numAabbChecks++; + bool nodeOverlap = b3TestAabbAgainstAabb(aabbAMinOut,aabbAMaxOut,aabbBMinOut,aabbBMaxOut); + if (nodeOverlap) + { + bool isLeafA = treeNodesCPU[node.x].isLeafNode(); + bool isLeafB = treeNodesCPU[node.y].isLeafNode(); + bool isInternalA = !isLeafA; + bool isInternalB = !isLeafB; + + //fail, even though it might hit two leaf nodes + if (depth+4>maxStackDepth && !(isLeafA && isLeafB)) + { + b3Error("Error: traversal exceeded maxStackDepth\n"); + continue; + } + + if(isInternalA) + { + int nodeAleftChild = node.x+1; + bool isNodeALeftChildLeaf = treeNodesCPU[node.x+1].isLeafNode(); + int nodeArightChild = isNodeALeftChildLeaf? node.x+2 : node.x+1 + treeNodesCPU[node.x+1].getEscapeIndex(); + + if(isInternalB) + { + int nodeBleftChild = node.y+1; + bool isNodeBLeftChildLeaf = treeNodesCPU[node.y+1].isLeafNode(); + int nodeBrightChild = isNodeBLeftChildLeaf? node.y+2 : node.y+1 + treeNodesCPU[node.y+1].getEscapeIndex(); + + nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBleftChild); + nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBleftChild); + nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBrightChild); + nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBrightChild); + } + else + { + nodeStack[depth++] = b3MakeInt2(nodeAleftChild,node.y); + nodeStack[depth++] = b3MakeInt2(nodeArightChild,node.y); + } + } + else + { + if(isInternalB) + { + int nodeBleftChild = node.y+1; + bool isNodeBLeftChildLeaf = treeNodesCPU[node.y+1].isLeafNode(); + int nodeBrightChild = isNodeBLeftChildLeaf? node.y+2 : node.y+1 + treeNodesCPU[node.y+1].getEscapeIndex(); + nodeStack[depth++] = b3MakeInt2(node.x,nodeBleftChild); + nodeStack[depth++] = b3MakeInt2(node.x,nodeBrightChild); + } + else + { + int compoundPairIdx = b3AtomicInc(numCompoundPairsOut); + if (compoundPairIdx& vertices, + __global const b3AlignedObjectArray& uniqueEdges, + __global const b3AlignedObjectArray& faces, + __global const b3AlignedObjectArray& indices, + __global b3Aabb* aabbs, + __global const b3GpuChildShape* gpuChildShapes, + __global b3AlignedObjectArray& gpuCompoundSepNormalsOut, + __global b3AlignedObjectArray& gpuHasCompoundSepNormalsOut, + int numCompoundPairs, + int i + ) +{ + +// int i = get_global_id(0); + if (i= 0) + { + collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex; + float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition; + b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation; + float4 newPosA = b3QuatRotate(ornA,childPosA)+posA; + b3Quat newOrnA = b3QuatMul(ornA,childOrnA); + posA = newPosA; + ornA = newOrnA; + } else + { + collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + } + + if (childShapeIndexB>=0) + { + collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex; + float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition; + b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation; + float4 newPosB = b3QuatRotate(ornB,childPosB)+posB; + b3Quat newOrnB = b3QuatMul(ornB,childOrnB); + posB = newPosB; + ornB = newOrnB; + } else + { + collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + } + + gpuHasCompoundSepNormalsOut[i] = 0; + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + int shapeTypeA = collidables[collidableIndexA].m_shapeType; + int shapeTypeB = collidables[collidableIndexB].m_shapeType; + + + if ((shapeTypeA != SHAPE_CONVEX_HULL) || (shapeTypeB != SHAPE_CONVEX_HULL)) + { + return; + } + + int hasSeparatingAxis = 5; + + // int numFacesA = convexShapes[shapeIndexA].m_numFaces; + float dmin = FLT_MAX; + posA.w = 0.f; + posB.w = 0.f; + float4 c0local = convexShapes[shapeIndexA].m_localCenter; + float4 c0 = transform(&c0local, &posA, &ornA); + float4 c1local = convexShapes[shapeIndexB].m_localCenter; + float4 c1 = transform(&c1local,&posB,&ornB); + const float4 DeltaC2 = c0 - c1; + float4 sepNormal = make_float4(1,0,0,0); +// bool sepA = findSeparatingAxis( convexShapes[shapeIndexA], convexShapes[shapeIndexB],posA,ornA,posB,ornB,DeltaC2,vertices,uniqueEdges,faces,indices,&sepNormal,&dmin); + bool sepA = findSeparatingAxis( convexShapes[shapeIndexA], convexShapes[shapeIndexB],posA,ornA,posB,ornB,vertices,uniqueEdges,faces,indices,vertices,uniqueEdges,faces,indices,sepNormal);//,&dmin); + + hasSeparatingAxis = 4; + if (!sepA) + { + hasSeparatingAxis = 0; + } else + { + bool sepB = findSeparatingAxis( convexShapes[shapeIndexB],convexShapes[shapeIndexA],posB,ornB,posA,ornA,vertices,uniqueEdges,faces,indices,vertices,uniqueEdges,faces,indices,sepNormal);//,&dmin); + + if (!sepB) + { + hasSeparatingAxis = 0; + } else//(!sepB) + { + bool sepEE = findSeparatingAxisEdgeEdge( &convexShapes[shapeIndexA], &convexShapes[shapeIndexB],posA,ornA,posB,ornB,DeltaC2,vertices,uniqueEdges,faces,indices,&sepNormal,&dmin); + if (sepEE) + { + gpuCompoundSepNormalsOut[i] = sepNormal;//fastNormalize4(sepNormal); + gpuHasCompoundSepNormalsOut[i] = 1; + }//sepEE + }//(!sepB) + }//(!sepA) + + + } + +} + + +__kernel void clipCompoundsHullHullKernel( __global const b3Int4* gpuCompoundPairs, + __global const b3RigidBodyData* rigidBodies, + __global const b3Collidable* collidables, + __global const b3ConvexPolyhedronData* convexShapes, + __global const b3AlignedObjectArray& vertices, + __global const b3AlignedObjectArray& uniqueEdges, + __global const b3AlignedObjectArray& faces, + __global const b3AlignedObjectArray& indices, + __global const b3GpuChildShape* gpuChildShapes, + __global const b3AlignedObjectArray& gpuCompoundSepNormalsOut, + __global const b3AlignedObjectArray& gpuHasCompoundSepNormalsOut, + __global struct b3Contact4Data* globalContactsOut, + int* nGlobalContactsOut, + int numCompoundPairs, int maxContactCapacity, int i) +{ + +// int i = get_global_id(0); + int pairIndex = i; + + float4 worldVertsB1[64]; + float4 worldVertsB2[64]; + int capacityWorldVerts = 64; + + float4 localContactsOut[64]; + int localContactCapacity=64; + + float minDist = -1e30f; + float maxDist = 0.0f; + + if (i= 0) + { + collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex; + float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition; + b3Quat childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation; + float4 newPosA = b3QuatRotate(ornA,childPosA)+posA; + b3Quat newOrnA = b3QuatMul(ornA,childOrnA); + posA = newPosA; + ornA = newOrnA; + } else + { + collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + } + + if (childShapeIndexB>=0) + { + collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex; + float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition; + b3Quat childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation; + float4 newPosB = b3QuatRotate(ornB,childPosB)+posB; + b3Quat newOrnB = b3QuatMul(ornB,childOrnB); + posB = newPosB; + ornB = newOrnB; + } else + { + collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + } + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + int numLocalContactsOut = clipHullAgainstHull(gpuCompoundSepNormalsOut[i], + convexShapes[shapeIndexA], convexShapes[shapeIndexB], + posA,ornA, + posB,ornB, + worldVertsB1,worldVertsB2,capacityWorldVerts, + minDist, maxDist, + vertices,faces,indices, + vertices,faces,indices, + localContactsOut,localContactCapacity); + + if (numLocalContactsOut>0) + { + float4 normal = -gpuCompoundSepNormalsOut[i]; + int nPoints = numLocalContactsOut; + float4* pointsIn = localContactsOut; + b3Int4 contactIdx;// = {-1,-1,-1,-1}; + + contactIdx.s[0] = 0; + contactIdx.s[1] = 1; + contactIdx.s[2] = 2; + contactIdx.s[3] = 3; + + int nReducedContacts = extractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx); + + int dstIdx; + dstIdx = b3AtomicInc( nGlobalContactsOut); + if ((dstIdx+nReducedContacts) < maxContactCapacity) + { + __global struct b3Contact4Data* c = globalContactsOut+ dstIdx; + c->m_worldNormalOnB = -normal; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + int bodyA = gpuCompoundPairs[pairIndex].x; + int bodyB = gpuCompoundPairs[pairIndex].y; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB; + c->m_childIndexA = childShapeIndexA; + c->m_childIndexB = childShapeIndexB; + for (int i=0;im_worldPosB[i] = pointsIn[contactIdx.s[i]]; + } + b3Contact4Data_setNumPoints(c,nReducedContacts); + } + + }// if (numContactsOut>0) + }// if (gpuHasCompoundSepNormalsOut[i]) + }// if (i& hostAabbsWorldSpace, + const b3AlignedObjectArray& hostAabbsLocalSpace, + + const b3AlignedObjectArray& convexVertices, + const b3AlignedObjectArray& hostUniqueEdges, + const b3AlignedObjectArray& convexIndices, + const b3AlignedObjectArray& faces, + + b3Contact4* globalContactsOut, + int& nGlobalContactsOut, + int maxContactCapacity, + b3AlignedObjectArray& treeNodesCPU, + b3AlignedObjectArray& subTreesCPU, + b3AlignedObjectArray& bvhInfoCPU + ) +{ + + int shapeTypeB = collidables[collidableIndexB].m_shapeType; + b3Assert(shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS); + + b3AlignedObjectArray cpuCompoundPairsOut; + int numCompoundPairsOut=0; + int maxNumCompoundPairsCapacity = 8192;//1024; + cpuCompoundPairsOut.resize(maxNumCompoundPairsCapacity); + + // work-in-progress + findCompoundPairsKernel( + pairIndex, + bodyIndexA,bodyIndexB, + collidableIndexA,collidableIndexB, + rigidBodies, + collidables, + convexShapes, + convexVertices, + hostAabbsWorldSpace, + hostAabbsLocalSpace, + cpuChildShapes, + &cpuCompoundPairsOut[0], + &numCompoundPairsOut, + maxNumCompoundPairsCapacity , + treeNodesCPU, + subTreesCPU, + bvhInfoCPU + ); + + printf("maxNumAabbChecks=%d\n",maxNumAabbChecks); + if (numCompoundPairsOut>maxNumCompoundPairsCapacity) + { + b3Error("numCompoundPairsOut exceeded maxNumCompoundPairsCapacity (%d)\n",maxNumCompoundPairsCapacity); + numCompoundPairsOut=maxNumCompoundPairsCapacity; + } + b3AlignedObjectArray cpuCompoundSepNormalsOut; + b3AlignedObjectArray cpuHasCompoundSepNormalsOut; + cpuCompoundSepNormalsOut.resize(numCompoundPairsOut); + cpuHasCompoundSepNormalsOut.resize(numCompoundPairsOut); + + for (int i=0;im_numVertices;i++) + { + b3Vector3 vtx = convexVertices[hullB->m_vertexOffset+i]; + float curDot = vtx.dot(planeNormalInConvex); + + + if (curDot>maxDot) + { + hitVertex=i; + maxDot=curDot; + hitVtx = vtx; + //make sure the deepest points is always included + if (numPoints==MAX_PLANE_CONVEX_POINTS) + numPoints--; + } + + if (numPoints4) + { + numReducedPoints = extractManifoldSequentialGlobal( contactPoints, numPoints, planeNormalInConvex, &contactIdx); + } + int dstIdx; + // dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx ); + + if (numReducedPoints>0) + { + if (nGlobalContactsOut < maxContactCapacity) + { + dstIdx=nGlobalContactsOut; + nGlobalContactsOut++; + + b3Contact4* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = -planeNormalWorld; + c->setFrictionCoeff(0.7); + c->setRestituitionCoeff(0.f); + + c->m_batchIdx = pairIndex; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB; + for (int i=0;im_worldPosB[i] = pOnB1; + } + c->m_worldNormalOnB.w = (b3Scalar)numReducedPoints; + }//if (dstIdx < numPairs) + } + + } + + +} + + + + + +void computeContactSphereConvex(int pairIndex, + int bodyIndexA, int bodyIndexB, + int collidableIndexA, int collidableIndexB, + const b3RigidBodyData* rigidBodies, + const b3Collidable* collidables, + const b3ConvexPolyhedronData* convexShapes, + const b3Vector3* convexVertices, + const int* convexIndices, + const b3GpuFace* faces, + b3Contact4* globalContactsOut, + int& nGlobalContactsOut, + int maxContactCapacity) +{ + + float radius = collidables[collidableIndexA].m_radius; + float4 spherePos1 = rigidBodies[bodyIndexA].m_pos; + b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat; + + + + float4 pos = rigidBodies[bodyIndexB].m_pos; + + + b3Quaternion quat = rigidBodies[bodyIndexB].m_quat; + + b3Transform tr; + tr.setIdentity(); + tr.setOrigin(pos); + tr.setRotation(quat); + b3Transform trInv = tr.inverse(); + + float4 spherePos = trInv(spherePos1); + + int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx; + int shapeIndex = collidables[collidableIndex].m_shapeIndex; + int numFaces = convexShapes[shapeIndex].m_numFaces; + float4 closestPnt = b3MakeVector3(0, 0, 0, 0); +// float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0); + float minDist = -1000000.f; // TODO: What is the largest/smallest float? + bool bCollide = true; + int region = -1; + float4 localHitNormal; + for ( int f = 0; f < numFaces; f++ ) + { + b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f]; + float4 planeEqn; + float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); + float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal); + planeEqn = n1; + planeEqn[3] = face.m_plane.w; + + float4 pntReturn; + float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn); + + if ( dist > radius) + { + bCollide = false; + break; + } + + if ( dist > 0 ) + { + //might hit an edge or vertex + b3Vector3 out; + + bool isInPoly = IsPointInPolygon(spherePos, + &face, + &convexVertices[convexShapes[shapeIndex].m_vertexOffset], + convexIndices, + &out); + if (isInPoly) + { + if (dist>minDist) + { + minDist = dist; + closestPnt = pntReturn; + localHitNormal = planeEqn; + region=1; + } + } else + { + b3Vector3 tmp = spherePos-out; + b3Scalar l2 = tmp.length2(); + if (l2minDist) + { + minDist = dist; + closestPnt = out; + localHitNormal = tmp/dist; + region=2; + } + + } else + { + bCollide = false; + break; + } + } + } + else + { + if ( dist > minDist ) + { + minDist = dist; + closestPnt = pntReturn; + localHitNormal = planeEqn; + region=3; + } + } + } + static int numChecks = 0; + numChecks++; + + if (bCollide && minDist > -10000) + { + + float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld; + float4 pOnB1 = tr(closestPnt); + //printf("dist ,%f,",minDist); + float actualDepth = minDist-radius; + if (actualDepth<0) + { + //printf("actualDepth = ,%f,", actualDepth); + //printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z); + //printf("region=,%d,\n", region); + pOnB1[3] = actualDepth; + + int dstIdx; +// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx ); + + if (nGlobalContactsOut < maxContactCapacity) + { + dstIdx=nGlobalContactsOut; + nGlobalContactsOut++; + + b3Contact4* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = normalOnSurfaceB1; + c->setFrictionCoeff(0.7); + c->setRestituitionCoeff(0.f); + + c->m_batchIdx = pairIndex; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB; + c->m_worldPosB[0] = pOnB1; + int numPoints = 1; + c->m_worldNormalOnB.w = (b3Scalar)numPoints; + }//if (dstIdx < numPairs) + } + }//if (hasCollision) + +} + + + + +int computeContactConvexConvex2( + int pairIndex, + int bodyIndexA, int bodyIndexB, + int collidableIndexA, int collidableIndexB, + const b3AlignedObjectArray& rigidBodies, + const b3AlignedObjectArray& collidables, + const b3AlignedObjectArray& convexShapes, + const b3AlignedObjectArray& convexVertices, + const b3AlignedObjectArray& uniqueEdges, + const b3AlignedObjectArray& convexIndices, + const b3AlignedObjectArray& faces, + b3AlignedObjectArray& globalContactsOut, + int& nGlobalContactsOut, + int maxContactCapacity, + const b3AlignedObjectArray& oldContacts + ) +{ + int contactIndex = -1; + b3Vector3 posA = rigidBodies[bodyIndexA].m_pos; + b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat; + b3Vector3 posB = rigidBodies[bodyIndexB].m_pos; + b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat; + + + b3ConvexPolyhedronData hullA, hullB; + + b3Vector3 sepNormalWorldSpace; + + + + b3Collidable colA = collidables[collidableIndexA]; + hullA = convexShapes[colA.m_shapeIndex]; + //printf("numvertsA = %d\n",hullA.m_numVertices); + + + b3Collidable colB = collidables[collidableIndexB]; + hullB = convexShapes[colB.m_shapeIndex]; + //printf("numvertsB = %d\n",hullB.m_numVertices); + +// int contactCapacity = MAX_VERTS; + //int numContactsOut=0; + + +#ifdef _WIN32 + b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x)); + b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x)); +#endif + + bool foundSepAxis = findSeparatingAxis(hullA,hullB, + posA, + ornA, + posB, + ornB, + + convexVertices,uniqueEdges,faces,convexIndices, + convexVertices,uniqueEdges,faces,convexIndices, + + sepNormalWorldSpace + ); + + + if (foundSepAxis) + { + + + contactIndex = clipHullHullSingle( + bodyIndexA, bodyIndexB, + posA,ornA, + posB,ornB, + collidableIndexA, collidableIndexB, + &rigidBodies, + &globalContactsOut, + nGlobalContactsOut, + + convexShapes, + convexShapes, + + convexVertices, + uniqueEdges, + faces, + convexIndices, + + convexVertices, + uniqueEdges, + faces, + convexIndices, + + collidables, + collidables, + sepNormalWorldSpace, + maxContactCapacity); + + } + + return contactIndex; +} + + + + + + + +void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* pairs, int nPairs, + const b3OpenCLArray* bodyBuf, + b3OpenCLArray* contactOut, int& nContacts, + const b3OpenCLArray* oldContacts, + int maxContactCapacity, + int compoundPairCapacity, + const b3OpenCLArray& convexData, + const b3OpenCLArray& gpuVertices, + const b3OpenCLArray& gpuUniqueEdges, + const b3OpenCLArray& gpuFaces, + const b3OpenCLArray& gpuIndices, + const b3OpenCLArray& gpuCollidables, + const b3OpenCLArray& gpuChildShapes, + + const b3OpenCLArray& clAabbsWorldSpace, + const b3OpenCLArray& clAabbsLocalSpace, + + b3OpenCLArray& worldVertsB1GPU, + b3OpenCLArray& clippingFacesOutGPU, + b3OpenCLArray& worldNormalsAGPU, + b3OpenCLArray& worldVertsA1GPU, + b3OpenCLArray& worldVertsB2GPU, + b3AlignedObjectArray& bvhDataUnused, + b3OpenCLArray* treeNodesGPU, + b3OpenCLArray* subTreesGPU, + b3OpenCLArray* bvhInfo, + + int numObjects, + int maxTriConvexPairCapacity, + b3OpenCLArray& triangleConvexPairsOut, + int& numTriConvexPairsOut + ) +{ + myframecount++; + + if (!nPairs) + return; + +#ifdef CHECK_ON_HOST + + + b3AlignedObjectArray treeNodesCPU; + treeNodesGPU->copyToHost(treeNodesCPU); + + b3AlignedObjectArray subTreesCPU; + subTreesGPU->copyToHost(subTreesCPU); + + b3AlignedObjectArray bvhInfoCPU; + bvhInfo->copyToHost(bvhInfoCPU); + + b3AlignedObjectArray hostAabbsWorldSpace; + clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace); + + b3AlignedObjectArray hostAabbsLocalSpace; + clAabbsLocalSpace.copyToHost(hostAabbsLocalSpace); + + b3AlignedObjectArray hostPairs; + pairs->copyToHost(hostPairs); + + b3AlignedObjectArray hostBodyBuf; + bodyBuf->copyToHost(hostBodyBuf); + + + + b3AlignedObjectArray hostConvexData; + convexData.copyToHost(hostConvexData); + + b3AlignedObjectArray hostVertices; + gpuVertices.copyToHost(hostVertices); + + b3AlignedObjectArray hostUniqueEdges; + gpuUniqueEdges.copyToHost(hostUniqueEdges); + b3AlignedObjectArray hostFaces; + gpuFaces.copyToHost(hostFaces); + b3AlignedObjectArray hostIndices; + gpuIndices.copyToHost(hostIndices); + b3AlignedObjectArray hostCollidables; + gpuCollidables.copyToHost(hostCollidables); + + b3AlignedObjectArray cpuChildShapes; + gpuChildShapes.copyToHost(cpuChildShapes); + + + b3AlignedObjectArray hostTriangleConvexPairs; + + b3AlignedObjectArray hostContacts; + if (nContacts) + { + contactOut->copyToHost(hostContacts); + } + + b3AlignedObjectArray oldHostContacts; + + if (oldContacts->size()) + { + oldContacts->copyToHost(oldHostContacts); + } + + + hostContacts.resize(maxContactCapacity); + + for (int i=0;i=0) + { +// printf("convex convex contactIndex = %d\n",contactIndex); + hostPairs[i].z = contactIndex; + } +// printf("plane-convex\n"); + + } + + + } + + if (hostPairs.size()) + { + pairs->copyFromHost(hostPairs); + } + + hostContacts.resize(nContacts); + if (nContacts) + { + + contactOut->copyFromHost(hostContacts); + } else + { + contactOut->resize(0); + } + + m_totalContactsOut.copyFromHostPointer(&nContacts,1,0,true); + //printf("(HOST) nContacts = %d\n",nContacts); + +#else + + { + if (nPairs) + { + m_totalContactsOut.copyFromHostPointer(&nContacts,1,0,true); + + B3_PROFILE("primitiveContactsKernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( pairs->getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( contactOut->getBufferCL()), + b3BufferInfoCL( m_totalContactsOut.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_primitiveContactsKernel,"m_primitiveContactsKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( nPairs ); + launcher.setConst(maxContactCapacity); + int num = nPairs; + launcher.launch1D( num); + clFinish(m_queue); + + nContacts = m_totalContactsOut.at(0); + contactOut->resize(nContacts); + } + } + + +#endif//CHECK_ON_HOST + + B3_PROFILE("computeConvexConvexContactsGPUSAT"); + // printf("nContacts = %d\n",nContacts); + + + m_sepNormals.resize(nPairs); + m_hasSeparatingNormals.resize(nPairs); + + int concaveCapacity=maxTriConvexPairCapacity; + m_concaveSepNormals.resize(concaveCapacity); + m_concaveHasSeparatingNormals.resize(concaveCapacity); + m_numConcavePairsOut.resize(0); + m_numConcavePairsOut.push_back(0); + + + m_gpuCompoundPairs.resize(compoundPairCapacity); + + m_gpuCompoundSepNormals.resize(compoundPairCapacity); + + + m_gpuHasCompoundSepNormals.resize(compoundPairCapacity); + + m_numCompoundPairsOut.resize(0); + m_numCompoundPairsOut.push_back(0); + + int numCompoundPairs = 0; + + int numConcavePairs =0; + + { + clFinish(m_queue); + if (findSeparatingAxisOnGpu) + { + m_dmins.resize(nPairs); + if (splitSearchSepAxisConvex) + { + + + if (useMprGpu) + { + nContacts = m_totalContactsOut.at(0); + { + B3_PROFILE("mprPenetrationKernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( pairs->getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( m_sepNormals.getBufferCL()), + b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()), + b3BufferInfoCL( contactOut->getBufferCL()), + b3BufferInfoCL( m_totalContactsOut.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_mprPenetrationKernel,"mprPenetrationKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + + launcher.setConst(maxContactCapacity); + launcher.setConst( nPairs ); + + int num = nPairs; + launcher.launch1D( num); + clFinish(m_queue); + /* + b3AlignedObjectArrayhostHasSepAxis; + m_hasSeparatingNormals.copyToHost(hostHasSepAxis); + b3AlignedObjectArrayhostSepAxis; + m_sepNormals.copyToHost(hostSepAxis); + */ + nContacts = m_totalContactsOut.at(0); + contactOut->resize(nContacts); + // printf("nContacts (after mprPenetrationKernel) = %d\n",nContacts); + if (nContacts>maxContactCapacity) + { + + b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity); + nContacts = maxContactCapacity; + } + + } + } + + if (1) + { + + if (1) + { + { + B3_PROFILE("findSeparatingAxisVertexFaceKernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( pairs->getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true), + b3BufferInfoCL( m_sepNormals.getBufferCL()), + b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()), + b3BufferInfoCL( m_dmins.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_findSeparatingAxisVertexFaceKernel,"findSeparatingAxisVertexFaceKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( nPairs ); + + int num = nPairs; + launcher.launch1D( num); + clFinish(m_queue); + } + + + int numDirections = sizeof(unitSphere162)/sizeof(b3Vector3); + + { + B3_PROFILE("findSeparatingAxisEdgeEdgeKernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( pairs->getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true), + b3BufferInfoCL( m_sepNormals.getBufferCL()), + b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()), + b3BufferInfoCL( m_dmins.getBufferCL()), + b3BufferInfoCL( m_unitSphereDirections.getBufferCL(),true) + + }; + + b3LauncherCL launcher(m_queue, m_findSeparatingAxisEdgeEdgeKernel,"findSeparatingAxisEdgeEdgeKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( numDirections); + launcher.setConst( nPairs ); + int num = nPairs; + launcher.launch1D( num); + clFinish(m_queue); + + } + } + if (useMprGpu) + { + B3_PROFILE("findSeparatingAxisUnitSphereKernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( pairs->getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( m_unitSphereDirections.getBufferCL(),true), + b3BufferInfoCL( m_sepNormals.getBufferCL()), + b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()), + b3BufferInfoCL( m_dmins.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_findSeparatingAxisUnitSphereKernel,"findSeparatingAxisUnitSphereKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + int numDirections = sizeof(unitSphere162)/sizeof(b3Vector3); + launcher.setConst( numDirections); + + launcher.setConst( nPairs ); + + int num = nPairs; + launcher.launch1D( num); + clFinish(m_queue); + } + } + + + } else + { + B3_PROFILE("findSeparatingAxisKernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( pairs->getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true), + b3BufferInfoCL( m_sepNormals.getBufferCL()), + b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_findSeparatingAxisKernel,"m_findSeparatingAxisKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( nPairs ); + + int num = nPairs; + launcher.launch1D( num); + clFinish(m_queue); + } + + + } + else + { + + B3_PROFILE("findSeparatingAxisKernel CPU"); + + + b3AlignedObjectArray hostPairs; + pairs->copyToHost(hostPairs); + b3AlignedObjectArray hostBodyBuf; + bodyBuf->copyToHost(hostBodyBuf); + + b3AlignedObjectArray hostCollidables; + gpuCollidables.copyToHost(hostCollidables); + + b3AlignedObjectArray cpuChildShapes; + gpuChildShapes.copyToHost(cpuChildShapes); + + b3AlignedObjectArray hostConvexShapeData; + convexData.copyToHost(hostConvexShapeData); + + b3AlignedObjectArray hostVertices; + gpuVertices.copyToHost(hostVertices); + + b3AlignedObjectArray hostHasSepAxis; + hostHasSepAxis.resize(nPairs); + b3AlignedObjectArray hostSepAxis; + hostSepAxis.resize(nPairs); + + b3AlignedObjectArray hostUniqueEdges; + gpuUniqueEdges.copyToHost(hostUniqueEdges); + b3AlignedObjectArray hostFaces; + gpuFaces.copyToHost(hostFaces); + + b3AlignedObjectArray hostIndices; + gpuIndices.copyToHost(hostIndices); + + b3AlignedObjectArray hostContacts; + if (nContacts) + { + contactOut->copyToHost(hostContacts); + } + hostContacts.resize(maxContactCapacity); + int nGlobalContactsOut = nContacts; + + + for (int i=0;i dist) + { + float diff = depth - dist; + + static float maxdiff = 0.f; + if (maxdiff < diff) + { + maxdiff = diff; + printf("maxdiff = %20.10f\n",maxdiff); + } + } + } + if (depth > dmin) + { + b3Vector3 oldAxis = hostSepAxis[i]; + depth = dmin; + sepAxis2 = oldAxis; + } + + + + if(b3TestSepAxis( &hullA, &hullB, posA,ornA,posB,ornB,&sepAxis2, &hostVertices[0], &hostVertices[0],&dist)) + { + if (depth > dist) + { + float diff = depth - dist; + //printf("?diff = %f\n",diff ); + static float maxdiff = 0.f; + if (maxdiff < diff) + { + maxdiff = diff; + printf("maxdiff = %20.10f\n",maxdiff); + } + } + //this is used for SAT + //hostHasSepAxis[i] = 1; + //hostSepAxis[i] = sepAxis2; + + //add contact point + + //int contactIndex = nGlobalContactsOut; + b3Contact4& newContact = hostContacts.at(nGlobalContactsOut); + nGlobalContactsOut++; + newContact.m_batchIdx = 0;//i; + newContact.m_bodyAPtrAndSignBit = (hostBodyBuf.at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA; + newContact.m_bodyBPtrAndSignBit = (hostBodyBuf.at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB; + + newContact.m_frictionCoeffCmp = 45874; + newContact.m_restituitionCoeffCmp = 0; + + + static float maxDepth = 0.f; + + if (depth > maxDepth) + { + maxDepth = depth; + printf("MPR maxdepth = %f\n",maxDepth ); + + } + + + resultPointOnBWorld.w = -depth; + newContact.m_worldPosB[0] = resultPointOnBWorld; + //b3Vector3 resultPointOnAWorld = resultPointOnBWorld+depth*sepAxis2; + newContact.m_worldNormalOnB = sepAxis2; + newContact.m_worldNormalOnB.w = (b3Scalar)1; + } else + { + printf("rejected\n"); + } + + + } + } else + { + + + + //int contactIndex = computeContactConvexConvex2( i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf, hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts); + b3AlignedObjectArray oldHostContacts; + int result; + result = computeContactConvexConvex2( //hostPairs, + pairIndex, + bodyIndexA, bodyIndexB, + collidableIndexA, collidableIndexB, + hostBodyBuf, + hostCollidables, + hostConvexShapeData, + hostVertices, + hostUniqueEdges, + hostIndices, + hostFaces, + hostContacts, + nGlobalContactsOut, + maxContactCapacity, + oldHostContacts + //hostHasSepAxis, + //hostSepAxis + + ); + }//mpr + }//hostHasSepAxis[i] = 1; + + } else + { + + b3Vector3 c0local = hostConvexShapeData[shapeIndexA].m_localCenter; + b3Vector3 c0 = b3TransformPoint(c0local, posA, ornA); + b3Vector3 c1local = hostConvexShapeData[shapeIndexB].m_localCenter; + b3Vector3 c1 = b3TransformPoint(c1local,posB,ornB); + b3Vector3 DeltaC2 = c0 - c1; + + b3Vector3 sepAxis; + + bool hasSepAxisA = b3FindSeparatingAxis(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2, + &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0), + &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0), + &sepAxis, &dmin); + + if (hasSepAxisA) + { + bool hasSepAxisB = b3FindSeparatingAxis(convexShapeB, convexShapeA, posB, ornB, posA, ornA, DeltaC2, + &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0), + &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0), + &sepAxis, &dmin); + if (hasSepAxisB) + { + bool hasEdgeEdge =b3FindSeparatingAxisEdgeEdge(convexShapeA, convexShapeB, posA, ornA, posB, ornB, DeltaC2, + &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0), + &hostVertices.at(0), &hostUniqueEdges.at(0), &hostFaces.at(0), &hostIndices.at(0), + &sepAxis, &dmin,true); + + if (hasEdgeEdge) + { + hostHasSepAxis[i] = 1; + hostSepAxis[i] = sepAxis; + } + } + } + } + } + + if (useGjkContacts)//nGlobalContactsOut>0) + { + //printf("nGlobalContactsOut=%d\n",nGlobalContactsOut); + nContacts = nGlobalContactsOut; + contactOut->copyFromHost(hostContacts); + + m_totalContactsOut.copyFromHostPointer(&nContacts,1,0,true); + } + + m_hasSeparatingNormals.copyFromHost(hostHasSepAxis); + m_sepNormals.copyFromHost(hostSepAxis); + + /* + //double-check results from GPU (comment-out the 'else' so both paths are executed + b3AlignedObjectArray checkHasSepAxis; + m_hasSeparatingNormals.copyToHost(checkHasSepAxis); + static int frameCount = 0; + frameCount++; + for (int i=0;igetBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( clAabbsLocalSpace.getBufferCL(),true), + b3BufferInfoCL( gpuChildShapes.getBufferCL(),true), + b3BufferInfoCL( m_gpuCompoundPairs.getBufferCL()), + b3BufferInfoCL( m_numCompoundPairsOut.getBufferCL()), + b3BufferInfoCL(subTreesGPU->getBufferCL()), + b3BufferInfoCL(treeNodesGPU->getBufferCL()), + b3BufferInfoCL(bvhInfo->getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_findCompoundPairsKernel,"m_findCompoundPairsKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( nPairs ); + launcher.setConst( compoundPairCapacity); + + int num = nPairs; + launcher.launch1D( num); + clFinish(m_queue); + + numCompoundPairs = m_numCompoundPairsOut.at(0); + //printf("numCompoundPairs =%d\n",numCompoundPairs ); + if (numCompoundPairs) + { + //printf("numCompoundPairs=%d\n",numCompoundPairs); + } + + + } else + { + + + b3AlignedObjectArray treeNodesCPU; + treeNodesGPU->copyToHost(treeNodesCPU); + + b3AlignedObjectArray subTreesCPU; + subTreesGPU->copyToHost(subTreesCPU); + + b3AlignedObjectArray bvhInfoCPU; + bvhInfo->copyToHost(bvhInfoCPU); + + b3AlignedObjectArray hostAabbsWorldSpace; + clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace); + + b3AlignedObjectArray hostAabbsLocalSpace; + clAabbsLocalSpace.copyToHost(hostAabbsLocalSpace); + + b3AlignedObjectArray hostPairs; + pairs->copyToHost(hostPairs); + + b3AlignedObjectArray hostBodyBuf; + bodyBuf->copyToHost(hostBodyBuf); + + + b3AlignedObjectArray cpuCompoundPairsOut; + cpuCompoundPairsOut.resize(compoundPairCapacity); + + b3AlignedObjectArray hostCollidables; + gpuCollidables.copyToHost(hostCollidables); + + b3AlignedObjectArray cpuChildShapes; + gpuChildShapes.copyToHost(cpuChildShapes); + + b3AlignedObjectArray hostConvexData; + convexData.copyToHost(hostConvexData); + + b3AlignedObjectArray hostVertices; + gpuVertices.copyToHost(hostVertices); + + + + + for (int pairIndex=0;pairIndex compoundPairCapacity) + { + b3Error("Exceeded compound pair capacity (%d/%d)\n", numCompoundPairs, compoundPairCapacity); + numCompoundPairs = compoundPairCapacity; + } + + + + m_gpuCompoundPairs.resize(numCompoundPairs); + m_gpuHasCompoundSepNormals.resize(numCompoundPairs); + m_gpuCompoundSepNormals.resize(numCompoundPairs); + + + if (numCompoundPairs) + { + B3_PROFILE("processCompoundPairsPrimitivesKernel"); + b3BufferInfoCL bInfo[] = + { + b3BufferInfoCL( m_gpuCompoundPairs.getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true), + b3BufferInfoCL( gpuChildShapes.getBufferCL(),true), + b3BufferInfoCL( contactOut->getBufferCL()), + b3BufferInfoCL( m_totalContactsOut.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_processCompoundPairsPrimitivesKernel,"m_processCompoundPairsPrimitivesKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( numCompoundPairs ); + launcher.setConst(maxContactCapacity); + + int num = numCompoundPairs; + launcher.launch1D( num); + clFinish(m_queue); + nContacts = m_totalContactsOut.at(0); + //printf("nContacts (after processCompoundPairsPrimitivesKernel) = %d\n",nContacts); + if (nContacts>maxContactCapacity) + { + + b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity); + nContacts = maxContactCapacity; + } + } + + + if (numCompoundPairs) + { + B3_PROFILE("processCompoundPairsKernel"); + b3BufferInfoCL bInfo[] = + { + b3BufferInfoCL( m_gpuCompoundPairs.getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true), + b3BufferInfoCL( gpuChildShapes.getBufferCL(),true), + b3BufferInfoCL( m_gpuCompoundSepNormals.getBufferCL()), + b3BufferInfoCL( m_gpuHasCompoundSepNormals.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_processCompoundPairsKernel,"m_processCompoundPairsKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( numCompoundPairs ); + + int num = numCompoundPairs; + launcher.launch1D( num); + clFinish(m_queue); + + } + + + //printf("numConcave = %d\n",numConcave); + + + +// printf("hostNormals.size()=%d\n",hostNormals.size()); + //int numPairs = pairCount.at(0); + + + + } + int vertexFaceCapacity = 64; + + + + { + //now perform the tree query on GPU + + + + + if (treeNodesGPU->size() && treeNodesGPU->size()) + { + if (bvhTraversalKernelGPU) + { + + B3_PROFILE("m_bvhTraversalKernel"); + + + numConcavePairs = m_numConcavePairsOut.at(0); + + b3LauncherCL launcher(m_queue, m_bvhTraversalKernel,"m_bvhTraversalKernel"); + launcher.setBuffer( pairs->getBufferCL()); + launcher.setBuffer( bodyBuf->getBufferCL()); + launcher.setBuffer( gpuCollidables.getBufferCL()); + launcher.setBuffer( clAabbsWorldSpace.getBufferCL()); + launcher.setBuffer( triangleConvexPairsOut.getBufferCL()); + launcher.setBuffer( m_numConcavePairsOut.getBufferCL()); + launcher.setBuffer( subTreesGPU->getBufferCL()); + launcher.setBuffer( treeNodesGPU->getBufferCL()); + launcher.setBuffer( bvhInfo->getBufferCL()); + + launcher.setConst( nPairs ); + launcher.setConst( maxTriConvexPairCapacity); + int num = nPairs; + launcher.launch1D( num); + clFinish(m_queue); + numConcavePairs = m_numConcavePairsOut.at(0); + } else + { + b3AlignedObjectArray hostPairs; + pairs->copyToHost(hostPairs); + b3AlignedObjectArray hostBodyBuf; + bodyBuf->copyToHost(hostBodyBuf); + b3AlignedObjectArray hostCollidables; + gpuCollidables.copyToHost(hostCollidables); + b3AlignedObjectArray hostAabbsWorldSpace; + clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace); + + //int maxTriConvexPairCapacity, + b3AlignedObjectArray triangleConvexPairsOutHost; + triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity); + + //int numTriConvexPairsOutHost=0; + numConcavePairs = 0; + //m_numConcavePairsOut + + b3AlignedObjectArray treeNodesCPU; + treeNodesGPU->copyToHost(treeNodesCPU); + b3AlignedObjectArray subTreesCPU; + subTreesGPU->copyToHost(subTreesCPU); + b3AlignedObjectArray bvhInfoCPU; + bvhInfo->copyToHost(bvhInfoCPU); + //compute it... + + volatile int hostNumConcavePairsOut=0; + + // + for (int i=0;i maxTriConvexPairCapacity) + { + static int exceeded_maxTriConvexPairCapacity_count = 0; + b3Error("Exceeded the maxTriConvexPairCapacity (found %d but max is %d, it happened %d times)\n", + numConcavePairs,maxTriConvexPairCapacity,exceeded_maxTriConvexPairCapacity_count++); + numConcavePairs = maxTriConvexPairCapacity; + } + triangleConvexPairsOut.resize(numConcavePairs); + + if (numConcavePairs) + { + + + + + clippingFacesOutGPU.resize(numConcavePairs); + worldNormalsAGPU.resize(numConcavePairs); + worldVertsA1GPU.resize(vertexFaceCapacity*(numConcavePairs)); + worldVertsB1GPU.resize(vertexFaceCapacity*(numConcavePairs)); + + + if (findConcaveSeparatingAxisKernelGPU) + { + + /* + m_concaveHasSeparatingNormals.copyFromHost(concaveHasSeparatingNormalsCPU); + clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU); + worldVertsA1GPU.copyFromHost(worldVertsA1CPU); + worldNormalsAGPU.copyFromHost(worldNormalsACPU); + worldVertsB1GPU.copyFromHost(worldVertsB1CPU); + */ + + //now perform a SAT test for each triangle-convex element (stored in triangleConvexPairsOut) + if (splitSearchSepAxisConcave) + { + //printf("numConcavePairs = %d\n",numConcavePairs); + m_dmins.resize(numConcavePairs); + { + B3_PROFILE("findConcaveSeparatingAxisVertexFaceKernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( triangleConvexPairsOut.getBufferCL() ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( gpuChildShapes.getBufferCL(),true), + b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true), + b3BufferInfoCL( m_concaveSepNormals.getBufferCL()), + b3BufferInfoCL( m_concaveHasSeparatingNormals.getBufferCL()), + b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()), + b3BufferInfoCL( worldVertsA1GPU.getBufferCL()), + b3BufferInfoCL(worldNormalsAGPU.getBufferCL()), + b3BufferInfoCL(worldVertsB1GPU.getBufferCL()), + b3BufferInfoCL(m_dmins.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisVertexFaceKernel,"m_findConcaveSeparatingAxisVertexFaceKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(vertexFaceCapacity); + launcher.setConst( numConcavePairs ); + + int num = numConcavePairs; + launcher.launch1D( num); + clFinish(m_queue); + + + } +// numConcavePairs = 0; + if (1) + { + B3_PROFILE("findConcaveSeparatingAxisEdgeEdgeKernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( triangleConvexPairsOut.getBufferCL() ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( gpuChildShapes.getBufferCL(),true), + b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true), + b3BufferInfoCL( m_concaveSepNormals.getBufferCL()), + b3BufferInfoCL( m_concaveHasSeparatingNormals.getBufferCL()), + b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()), + b3BufferInfoCL( worldVertsA1GPU.getBufferCL()), + b3BufferInfoCL(worldNormalsAGPU.getBufferCL()), + b3BufferInfoCL(worldVertsB1GPU.getBufferCL()), + b3BufferInfoCL(m_dmins.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisEdgeEdgeKernel,"m_findConcaveSeparatingAxisEdgeEdgeKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(vertexFaceCapacity); + launcher.setConst( numConcavePairs ); + + int num = numConcavePairs; + launcher.launch1D( num); + clFinish(m_queue); + } + + + // numConcavePairs = 0; + + + + + + + } else + { + B3_PROFILE("findConcaveSeparatingAxisKernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( triangleConvexPairsOut.getBufferCL() ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( gpuChildShapes.getBufferCL(),true), + b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true), + b3BufferInfoCL( m_concaveSepNormals.getBufferCL()), + b3BufferInfoCL( m_concaveHasSeparatingNormals.getBufferCL()), + b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()), + b3BufferInfoCL( worldVertsA1GPU.getBufferCL()), + b3BufferInfoCL(worldNormalsAGPU.getBufferCL()), + b3BufferInfoCL(worldVertsB1GPU.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisKernel,"m_findConcaveSeparatingAxisKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(vertexFaceCapacity); + launcher.setConst( numConcavePairs ); + + int num = numConcavePairs; + launcher.launch1D( num); + clFinish(m_queue); + } + + + } else + { + + b3AlignedObjectArray clippingFacesOutCPU; + b3AlignedObjectArray worldVertsA1CPU; + b3AlignedObjectArray worldNormalsACPU; + b3AlignedObjectArray worldVertsB1CPU; + b3AlignedObjectArrayconcaveHasSeparatingNormalsCPU; + + b3AlignedObjectArray triangleConvexPairsOutHost; + triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost); + //triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity); + b3AlignedObjectArray hostBodyBuf; + bodyBuf->copyToHost(hostBodyBuf); + b3AlignedObjectArray hostCollidables; + gpuCollidables.copyToHost(hostCollidables); + b3AlignedObjectArray hostAabbsWorldSpace; + clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace); + + b3AlignedObjectArray hostConvexData; + convexData.copyToHost(hostConvexData); + + b3AlignedObjectArray hostVertices; + gpuVertices.copyToHost(hostVertices); + + b3AlignedObjectArray hostUniqueEdges; + gpuUniqueEdges.copyToHost(hostUniqueEdges); + b3AlignedObjectArray hostFaces; + gpuFaces.copyToHost(hostFaces); + b3AlignedObjectArray hostIndices; + gpuIndices.copyToHost(hostIndices); + b3AlignedObjectArray cpuChildShapes; + gpuChildShapes.copyToHost(cpuChildShapes); + + + + b3AlignedObjectArray concaveSepNormalsHost; + m_concaveSepNormals.copyToHost(concaveSepNormalsHost); + concaveHasSeparatingNormalsCPU.resize(concaveSepNormalsHost.size()); + + b3GpuChildShape* childShapePointerCPU = 0; + if (cpuChildShapes.size()) + childShapePointerCPU = &cpuChildShapes.at(0); + + clippingFacesOutCPU.resize(clippingFacesOutGPU.size()); + worldVertsA1CPU.resize(worldVertsA1GPU.size()); + worldNormalsACPU.resize(worldNormalsAGPU.size()); + worldVertsB1CPU.resize(worldVertsB1GPU.size()); + + for (int i=0;i cpuCompoundSepNormals; +// m_concaveSepNormals.copyToHost(cpuCompoundSepNormals); +// b3AlignedObjectArray cpuConcavePairs; +// triangleConvexPairsOut.copyToHost(cpuConcavePairs); + + + } + } + + + } + + if (numConcavePairs) + { + if (numConcavePairs) + { + B3_PROFILE("findConcaveSphereContactsKernel"); + nContacts = m_totalContactsOut.at(0); +// printf("nContacts1 = %d\n",nContacts); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( triangleConvexPairsOut.getBufferCL() ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true), + b3BufferInfoCL( contactOut->getBufferCL()), + b3BufferInfoCL( m_totalContactsOut.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_findConcaveSphereContactsKernel,"m_findConcaveSphereContactsKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + + launcher.setConst( numConcavePairs ); + launcher.setConst(maxContactCapacity); + + int num = numConcavePairs; + launcher.launch1D( num); + clFinish(m_queue); + nContacts = m_totalContactsOut.at(0); + //printf("nContacts (after findConcaveSphereContactsKernel) = %d\n",nContacts); + + //printf("nContacts2 = %d\n",nContacts); + + if (nContacts >= maxContactCapacity) + { + b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity); + nContacts = maxContactCapacity; + } + } + + } + + + +#ifdef __APPLE__ + bool contactClippingOnGpu = true; +#else + bool contactClippingOnGpu = true; +#endif + + if (contactClippingOnGpu) + { + m_totalContactsOut.copyFromHostPointer(&nContacts,1,0,true); +// printf("nContacts3 = %d\n",nContacts); + + + //B3_PROFILE("clipHullHullKernel"); + + bool breakupConcaveConvexKernel = true; + +#ifdef __APPLE__ + //actually, some Apple OpenCL platform/device combinations work fine... + breakupConcaveConvexKernel = true; +#endif + //concave-convex contact clipping + if (numConcavePairs) + { + // printf("numConcavePairs = %d\n", numConcavePairs); + // nContacts = m_totalContactsOut.at(0); + // printf("nContacts before = %d\n", nContacts); + + if (breakupConcaveConvexKernel) + { + + worldVertsB2GPU.resize(vertexFaceCapacity*numConcavePairs); + + + //clipFacesAndFindContacts + + if (clipConcaveFacesAndFindContactsCPU) + { + + b3AlignedObjectArray clippingFacesOutCPU; + b3AlignedObjectArray worldVertsA1CPU; + b3AlignedObjectArray worldNormalsACPU; + b3AlignedObjectArray worldVertsB1CPU; + + clippingFacesOutGPU.copyToHost(clippingFacesOutCPU); + worldVertsA1GPU.copyToHost(worldVertsA1CPU); + worldNormalsAGPU.copyToHost(worldNormalsACPU); + worldVertsB1GPU.copyToHost(worldVertsB1CPU); + + + + b3AlignedObjectArrayconcaveHasSeparatingNormalsCPU; + m_concaveHasSeparatingNormals.copyToHost(concaveHasSeparatingNormalsCPU); + + b3AlignedObjectArray concaveSepNormalsHost; + m_concaveSepNormals.copyToHost(concaveSepNormalsHost); + + b3AlignedObjectArray worldVertsB2CPU; + worldVertsB2CPU.resize(worldVertsB2GPU.size()); + + + for (int i=0;ireserve(newContactCapacity); + if (reduceConcaveContactsOnGPU) + { +// printf("newReservation = %d\n",newReservation); + { + B3_PROFILE("newContactReductionKernel"); + b3BufferInfoCL bInfo[] = + { + b3BufferInfoCL( triangleConvexPairsOut.getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( m_concaveSepNormals.getBufferCL()), + b3BufferInfoCL( m_concaveHasSeparatingNormals.getBufferCL()), + b3BufferInfoCL( contactOut->getBufferCL()), + b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()), + b3BufferInfoCL( worldVertsB2GPU.getBufferCL()), + b3BufferInfoCL( m_totalContactsOut.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_newContactReductionKernel,"m_newContactReductionKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(vertexFaceCapacity); + launcher.setConst(newContactCapacity); + launcher.setConst( numConcavePairs ); + int num = numConcavePairs; + + launcher.launch1D( num); + } + nContacts = m_totalContactsOut.at(0); + contactOut->resize(nContacts); + + //printf("contactOut4 (after newContactReductionKernel) = %d\n",nContacts); + }else + { + + volatile int nGlobalContactsOut = nContacts; + b3AlignedObjectArray triangleConvexPairsOutHost; + triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost); + b3AlignedObjectArray hostBodyBuf; + bodyBuf->copyToHost(hostBodyBuf); + + b3AlignedObjectArrayconcaveHasSeparatingNormalsCPU; + m_concaveHasSeparatingNormals.copyToHost(concaveHasSeparatingNormalsCPU); + + b3AlignedObjectArray concaveSepNormalsHost; + m_concaveSepNormals.copyToHost(concaveSepNormalsHost); + + + b3AlignedObjectArray hostContacts; + if (nContacts) + { + contactOut->copyToHost(hostContacts); + } + hostContacts.resize(newContactCapacity); + + b3AlignedObjectArray clippingFacesOutCPU; + b3AlignedObjectArray worldVertsB2CPU; + + clippingFacesOutGPU.copyToHost(clippingFacesOutCPU); + worldVertsB2GPU.copyToHost(worldVertsB2CPU); + + + + for (int i=0;iresize(nContacts); + hostContacts.resize(nContacts); + //printf("contactOut4 (after newContactReductionKernel) = %d\n",nContacts); + contactOut->copyFromHost(hostContacts); + } + + } + //re-use? + + + } else + { + B3_PROFILE("clipHullHullConcaveConvexKernel"); + nContacts = m_totalContactsOut.at(0); + int newContactCapacity = contactOut->capacity(); + + //printf("contactOut5 = %d\n",nContacts); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( triangleConvexPairsOut.getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( gpuChildShapes.getBufferCL(),true), + b3BufferInfoCL( m_concaveSepNormals.getBufferCL()), + b3BufferInfoCL( contactOut->getBufferCL()), + b3BufferInfoCL( m_totalContactsOut.getBufferCL()) + }; + b3LauncherCL launcher(m_queue, m_clipHullHullConcaveConvexKernel,"m_clipHullHullConcaveConvexKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(newContactCapacity); + launcher.setConst( numConcavePairs ); + int num = numConcavePairs; + launcher.launch1D( num); + clFinish(m_queue); + nContacts = m_totalContactsOut.at(0); + contactOut->resize(nContacts); + //printf("contactOut6 = %d\n",nContacts); + b3AlignedObjectArray cpuContacts; + contactOut->copyToHost(cpuContacts); + } + // printf("nContacts after = %d\n", nContacts); + }//numConcavePairs + + + + //convex-convex contact clipping + + bool breakupKernel = false; + +#ifdef __APPLE__ + breakupKernel = true; +#endif + +#ifdef CHECK_ON_HOST + bool computeConvexConvex = false; +#else + bool computeConvexConvex = true; +#endif//CHECK_ON_HOST + if (computeConvexConvex) + { + B3_PROFILE("clipHullHullKernel"); + if (breakupKernel) + { + + + + + worldVertsB1GPU.resize(vertexFaceCapacity*nPairs); + clippingFacesOutGPU.resize(nPairs); + worldNormalsAGPU.resize(nPairs); + worldVertsA1GPU.resize(vertexFaceCapacity*nPairs); + worldVertsB2GPU.resize(vertexFaceCapacity*nPairs); + + if (findConvexClippingFacesGPU) + { + B3_PROFILE("findClippingFacesKernel"); + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( pairs->getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( m_sepNormals.getBufferCL()), + b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()), + b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()), + b3BufferInfoCL( worldVertsA1GPU.getBufferCL()), + b3BufferInfoCL( worldNormalsAGPU.getBufferCL()), + b3BufferInfoCL( worldVertsB1GPU.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_findClippingFacesKernel,"m_findClippingFacesKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( vertexFaceCapacity); + launcher.setConst( nPairs ); + int num = nPairs; + launcher.launch1D( num); + clFinish(m_queue); + + } else + { + + float minDist = -1e30f; + float maxDist = 0.02f; + + b3AlignedObjectArray hostConvexData; + convexData.copyToHost(hostConvexData); + b3AlignedObjectArray hostCollidables; + gpuCollidables.copyToHost(hostCollidables); + + b3AlignedObjectArray hostHasSepNormals; + m_hasSeparatingNormals.copyToHost(hostHasSepNormals); + b3AlignedObjectArray cpuSepNormals; + m_sepNormals.copyToHost(cpuSepNormals); + + b3AlignedObjectArray hostPairs; + pairs->copyToHost(hostPairs); + b3AlignedObjectArray hostBodyBuf; + bodyBuf->copyToHost(hostBodyBuf); + + + //worldVertsB1GPU.resize(vertexFaceCapacity*nPairs); + b3AlignedObjectArray worldVertsB1CPU; + worldVertsB1GPU.copyToHost(worldVertsB1CPU); + + b3AlignedObjectArray clippingFacesOutCPU; + clippingFacesOutGPU.copyToHost(clippingFacesOutCPU); + + b3AlignedObjectArray worldNormalsACPU; + worldNormalsACPU.resize(nPairs); + + b3AlignedObjectArray worldVertsA1CPU; + worldVertsA1CPU.resize(worldVertsA1GPU.size()); + + + b3AlignedObjectArray hostVertices; + gpuVertices.copyToHost(hostVertices); + b3AlignedObjectArray hostFaces; + gpuFaces.copyToHost(hostFaces); + b3AlignedObjectArray hostIndices; + gpuIndices.copyToHost(hostIndices); + + + for (int i=0;i hostPairs; + //pairs->copyToHost(hostPairs); + + b3AlignedObjectArray hostSepNormals; + m_sepNormals.copyToHost(hostSepNormals); + b3AlignedObjectArray hostHasSepAxis; + m_hasSeparatingNormals.copyToHost(hostHasSepAxis); + + b3AlignedObjectArray hostClippingFaces; + clippingFacesOutGPU.copyToHost(hostClippingFaces); + b3AlignedObjectArray worldVertsB2CPU; + worldVertsB2CPU.resize(vertexFaceCapacity*nPairs); + + b3AlignedObjectArrayworldVertsA1CPU; + worldVertsA1GPU.copyToHost(worldVertsA1CPU); + b3AlignedObjectArray worldNormalsACPU; + worldNormalsAGPU.copyToHost(worldNormalsACPU); + + b3AlignedObjectArray worldVertsB1CPU; + worldVertsB1GPU.copyToHost(worldVertsB1CPU); + + /* + __global const b3Float4* separatingNormals, + __global const int* hasSeparatingAxis, + __global b3Int4* clippingFacesOut, + __global b3Float4* worldVertsA1, + __global b3Float4* worldNormalsA1, + __global b3Float4* worldVertsB1, + __global b3Float4* worldVertsB2, + int vertexFaceCapacity, + int pairIndex + */ + for (int i=0;ireserve(newContactCapacity); + + if (reduceConvexContactsOnGPU) + { + { + B3_PROFILE("newContactReductionKernel"); + b3BufferInfoCL bInfo[] = + { + b3BufferInfoCL( pairs->getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( m_sepNormals.getBufferCL()), + b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()), + b3BufferInfoCL( contactOut->getBufferCL()), + b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()), + b3BufferInfoCL( worldVertsB2GPU.getBufferCL()), + b3BufferInfoCL( m_totalContactsOut.getBufferCL()) + }; + + b3LauncherCL launcher(m_queue, m_newContactReductionKernel,"m_newContactReductionKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(vertexFaceCapacity); + launcher.setConst(newContactCapacity); + launcher.setConst( nPairs ); + int num = nPairs; + + launcher.launch1D( num); + } + nContacts = m_totalContactsOut.at(0); + contactOut->resize(nContacts); + } else + { + + volatile int nGlobalContactsOut = nContacts; + b3AlignedObjectArray hostPairs; + pairs->copyToHost(hostPairs); + b3AlignedObjectArray hostBodyBuf; + bodyBuf->copyToHost(hostBodyBuf); + b3AlignedObjectArray hostSepNormals; + m_sepNormals.copyToHost(hostSepNormals); + b3AlignedObjectArray hostHasSepAxis; + m_hasSeparatingNormals.copyToHost(hostHasSepAxis); + b3AlignedObjectArray hostContactsOut; + contactOut->copyToHost(hostContactsOut); + hostContactsOut.resize(newContactCapacity); + + b3AlignedObjectArray hostClippingFaces; + clippingFacesOutGPU.copyToHost(hostClippingFaces); + b3AlignedObjectArray worldVertsB2CPU; + worldVertsB2GPU.copyToHost(worldVertsB2CPU); + + for (int i=0;icopyFromHost(hostContactsOut); + } + // b3Contact4 pt = contactOut->at(0); + // printf("nContacts = %d\n",nContacts); + } + } + } + else//breakupKernel + { + + if (nPairs) + { + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( pairs->getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( m_sepNormals.getBufferCL()), + b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()), + b3BufferInfoCL( contactOut->getBufferCL()), + b3BufferInfoCL( m_totalContactsOut.getBufferCL()) + }; + b3LauncherCL launcher(m_queue, m_clipHullHullKernel,"m_clipHullHullKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( nPairs ); + launcher.setConst(maxContactCapacity); + + int num = nPairs; + launcher.launch1D( num); + clFinish(m_queue); + + nContacts = m_totalContactsOut.at(0); + if (nContacts >= maxContactCapacity) + { + b3Error("Exceeded contact capacity (%d/%d)\n",nContacts,maxContactCapacity); + nContacts = maxContactCapacity; + } + contactOut->resize(nContacts); + } + } + + + int nCompoundsPairs = m_gpuCompoundPairs.size(); + + if (nCompoundsPairs) + { + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( m_gpuCompoundPairs.getBufferCL(), true ), + b3BufferInfoCL( bodyBuf->getBufferCL(),true), + b3BufferInfoCL( gpuCollidables.getBufferCL(),true), + b3BufferInfoCL( convexData.getBufferCL(),true), + b3BufferInfoCL( gpuVertices.getBufferCL(),true), + b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true), + b3BufferInfoCL( gpuFaces.getBufferCL(),true), + b3BufferInfoCL( gpuIndices.getBufferCL(),true), + b3BufferInfoCL( gpuChildShapes.getBufferCL(),true), + b3BufferInfoCL( m_gpuCompoundSepNormals.getBufferCL(),true), + b3BufferInfoCL( m_gpuHasCompoundSepNormals.getBufferCL(),true), + b3BufferInfoCL( contactOut->getBufferCL()), + b3BufferInfoCL( m_totalContactsOut.getBufferCL()) + }; + b3LauncherCL launcher(m_queue, m_clipCompoundsHullHullKernel,"m_clipCompoundsHullHullKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( nCompoundsPairs ); + launcher.setConst(maxContactCapacity); + + int num = nCompoundsPairs; + launcher.launch1D( num); + clFinish(m_queue); + + nContacts = m_totalContactsOut.at(0); + if (nContacts>maxContactCapacity) + { + + b3Error("Error: contacts exceeds capacity (%d/%d)\n", nContacts, maxContactCapacity); + nContacts = maxContactCapacity; + } + contactOut->resize(nContacts); + }//if nCompoundsPairs + } + }//contactClippingOnGpu + + //printf("nContacts end = %d\n",nContacts); + + //printf("frameCount = %d\n",frameCount++); +} diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h new file mode 100644 index 000000000000..e24c1579c6dc --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h @@ -0,0 +1,118 @@ + +#ifndef _CONVEX_HULL_CONTACT_H +#define _CONVEX_HULL_CONTACT_H + +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" +#include "Bullet3Common/shared/b3Int2.h" +#include "Bullet3Common/shared/b3Int4.h" +#include "b3OptimizedBvh.h" +#include "b3BvhInfo.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" + +//#include "../../dynamics/basic_demo/Stubs/ChNarrowPhase.h" + + + + +struct GpuSatCollision +{ + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; + cl_kernel m_findSeparatingAxisKernel; + cl_kernel m_mprPenetrationKernel; + cl_kernel m_findSeparatingAxisUnitSphereKernel; + + + cl_kernel m_findSeparatingAxisVertexFaceKernel; + cl_kernel m_findSeparatingAxisEdgeEdgeKernel; + + cl_kernel m_findConcaveSeparatingAxisKernel; + cl_kernel m_findConcaveSeparatingAxisVertexFaceKernel; + cl_kernel m_findConcaveSeparatingAxisEdgeEdgeKernel; + + + + + cl_kernel m_findCompoundPairsKernel; + cl_kernel m_processCompoundPairsKernel; + + cl_kernel m_clipHullHullKernel; + cl_kernel m_clipCompoundsHullHullKernel; + + cl_kernel m_clipFacesAndFindContacts; + cl_kernel m_findClippingFacesKernel; + + cl_kernel m_clipHullHullConcaveConvexKernel; +// cl_kernel m_extractManifoldAndAddContactKernel; + cl_kernel m_newContactReductionKernel; + + cl_kernel m_bvhTraversalKernel; + cl_kernel m_primitiveContactsKernel; + cl_kernel m_findConcaveSphereContactsKernel; + + cl_kernel m_processCompoundPairsPrimitivesKernel; + + b3OpenCLArray m_unitSphereDirections; + + b3OpenCLArray m_totalContactsOut; + + b3OpenCLArray m_sepNormals; + b3OpenCLArray m_dmins; + + b3OpenCLArray m_hasSeparatingNormals; + b3OpenCLArray m_concaveSepNormals; + b3OpenCLArray m_concaveHasSeparatingNormals; + b3OpenCLArray m_numConcavePairsOut; + b3OpenCLArray m_gpuCompoundPairs; + b3OpenCLArray m_gpuCompoundSepNormals; + b3OpenCLArray m_gpuHasCompoundSepNormals; + b3OpenCLArray m_numCompoundPairsOut; + + + GpuSatCollision(cl_context ctx,cl_device_id device, cl_command_queue q ); + virtual ~GpuSatCollision(); + + + void computeConvexConvexContactsGPUSAT( b3OpenCLArray* pairs, int nPairs, + const b3OpenCLArray* bodyBuf, + b3OpenCLArray* contactOut, int& nContacts, + const b3OpenCLArray* oldContacts, + int maxContactCapacity, + int compoundPairCapacity, + const b3OpenCLArray& hostConvexData, + const b3OpenCLArray& vertices, + const b3OpenCLArray& uniqueEdges, + const b3OpenCLArray& faces, + const b3OpenCLArray& indices, + const b3OpenCLArray& gpuCollidables, + const b3OpenCLArray& gpuChildShapes, + + const b3OpenCLArray& clAabbsWorldSpace, + const b3OpenCLArray& clAabbsLocalSpace, + + b3OpenCLArray& worldVertsB1GPU, + b3OpenCLArray& clippingFacesOutGPU, + b3OpenCLArray& worldNormalsAGPU, + b3OpenCLArray& worldVertsA1GPU, + b3OpenCLArray& worldVertsB2GPU, + b3AlignedObjectArray& bvhData, + b3OpenCLArray* treeNodesGPU, + b3OpenCLArray* subTreesGPU, + b3OpenCLArray* bvhInfo, + int numObjects, + int maxTriConvexPairCapacity, + b3OpenCLArray& triangleConvexPairs, + int& numTriConvexPairsOut + ); + + +}; + +#endif //_CONVEX_HULL_CONTACT_H diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h new file mode 100644 index 000000000000..337100fb1a6a --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h @@ -0,0 +1,9 @@ +#ifndef CONVEX_POLYHEDRON_CL +#define CONVEX_POLYHEDRON_CL + +#include "Bullet3Common/b3Transform.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" + + + +#endif //CONVEX_POLYHEDRON_CL diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp new file mode 100644 index 000000000000..d636f983c69b --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp @@ -0,0 +1,1014 @@ +/* +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 "b3GjkEpa.h" + +#include "b3SupportMappings.h" + +namespace gjkepa2_impl2 +{ + + // Config + + /* GJK */ +#define GJK_MAX_ITERATIONS 128 +#define GJK_ACCURACY ((b3Scalar)0.0001) +#define GJK_MIN_DISTANCE ((b3Scalar)0.0001) +#define GJK_DUPLICATED_EPS ((b3Scalar)0.0001) +#define GJK_SIMPLEX2_EPS ((b3Scalar)0.0) +#define GJK_SIMPLEX3_EPS ((b3Scalar)0.0) +#define GJK_SIMPLEX4_EPS ((b3Scalar)0.0) + + /* EPA */ +#define EPA_MAX_VERTICES 64 +#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) +#define EPA_MAX_ITERATIONS 255 +#define EPA_ACCURACY ((b3Scalar)0.0001) +#define EPA_FALLBACK (10*EPA_ACCURACY) +#define EPA_PLANE_EPS ((b3Scalar)0.00001) +#define EPA_INSIDE_EPS ((b3Scalar)0.01) + + + // Shorthands + + + // MinkowskiDiff + struct b3MinkowskiDiff + { + + + const b3ConvexPolyhedronData* m_shapes[2]; + + + b3Matrix3x3 m_toshape1; + b3Transform m_toshape0; + + bool m_enableMargin; + + + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline b3Vector3 Support0(const b3Vector3& d, const b3AlignedObjectArray& verticesA) const + { + if (m_enableMargin) + { + return localGetSupportVertexWithMargin(d,m_shapes[0],verticesA,0.f); + } else + { + return localGetSupportVertexWithoutMargin(d,m_shapes[0],verticesA); + } + } + inline b3Vector3 Support1(const b3Vector3& d, const b3AlignedObjectArray& verticesB) const + { + if (m_enableMargin) + { + return m_toshape0*(localGetSupportVertexWithMargin(m_toshape1*d,m_shapes[1],verticesB,0.f)); + } else + { + return m_toshape0*(localGetSupportVertexWithoutMargin(m_toshape1*d,m_shapes[1],verticesB)); + } + } + + inline b3Vector3 Support(const b3Vector3& d, const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& verticesB) const + { + return(Support0(d,verticesA)-Support1(-d,verticesB)); + } + b3Vector3 Support(const b3Vector3& d,unsigned int index,const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& verticesB) const + { + if(index) + return(Support1(d,verticesA)); + else + return(Support0(d,verticesB)); + } + }; + + typedef b3MinkowskiDiff tShape; + + + // GJK + struct b3GJK + { + /* Types */ + struct sSV + { + b3Vector3 d,w; + }; + struct sSimplex + { + sSV* c[4]; + b3Scalar p[4]; + unsigned int rank; + }; + struct eStatus { enum _ { + Valid, + Inside, + Failed };}; + /* Fields */ + tShape m_shape; + const b3AlignedObjectArray& m_verticesA; + const b3AlignedObjectArray& m_verticesB; + b3Vector3 m_ray; + b3Scalar m_distance; + sSimplex m_simplices[2]; + sSV m_store[4]; + sSV* m_free[4]; + unsigned int m_nfree; + unsigned int m_current; + sSimplex* m_simplex; + eStatus::_ m_status; + /* Methods */ + b3GJK(const b3AlignedObjectArray& verticesA,const b3AlignedObjectArray& verticesB) + :m_verticesA(verticesA),m_verticesB(verticesB) + { + Initialize(); + } + void Initialize() + { + m_ray = b3MakeVector3(0,0,0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; + } + eStatus::_ Evaluate(const tShape& shapearg,const b3Vector3& guess) + { + unsigned int iterations=0; + b3Scalar sqdist=0; + b3Scalar alpha=0; + b3Vector3 lastw[4]; + unsigned int 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 b3Scalar sqrl= m_ray.length2(); + appendvertice(m_simplices[0],sqrl>0?-m_ray:b3MakeVector3(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 unsigned int next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const b3Scalar rl=m_ray.length(); + if(rlw; + bool found=false; + for(unsigned int i=0;i<4;++i) + { + if((w-lastw[i]).length2()w, + cs.c[1]->w, + weights,mask);break; + case 3: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights,mask);break; + case 4: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights,mask);break; + } + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = b3MakeVector3(0,0,0); + m_current = next; + for(unsigned int i=0,ni=cs.rank;iw*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) m_status=eStatus::Inside; + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)rank) + { + case 1: + { + for(unsigned int i=0;i<3;++i) + { + b3Vector3 axis=b3MakeVector3(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 b3Vector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; + for(unsigned int i=0;i<3;++i) + { + b3Vector3 axis=b3MakeVector3(0,0,0); + axis[i]=1; + const b3Vector3 p=b3Cross(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 b3Vector3 n=b3Cross(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(b3Fabs(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 b3Vector3& d,sSV& sv) const + { + sv.d = d/d.length(); + sv.w = m_shape.Support(sv.d,m_verticesA,m_verticesB); + } + void removevertice(sSimplex& simplex) + { + m_free[m_nfree++]=simplex.c[--simplex.rank]; + } + void appendvertice(sSimplex& simplex,const b3Vector3& v) + { + simplex.p[simplex.rank]=0; + simplex.c[simplex.rank]=m_free[--m_nfree]; + getsupport(v,*simplex.c[simplex.rank++]); + } + static b3Scalar det(const b3Vector3& a,const b3Vector3& b,const b3Vector3& 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 b3Scalar projectorigin( const b3Vector3& a, + const b3Vector3& b, + b3Scalar* w,unsigned int& m) + { + const b3Vector3 d=b-a; + const b3Scalar l=d.length2(); + if(l>GJK_SIMPLEX2_EPS) + { + const b3Scalar t(l>0?-b3Dot(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 b3Scalar projectorigin( const b3Vector3& a, + const b3Vector3& b, + const b3Vector3& c, + b3Scalar* w,unsigned int& m) + { + static const unsigned int imd3[]={1,2,0}; + const b3Vector3* vt[]={&a,&b,&c}; + const b3Vector3 dl[]={a-b,b-c,c-a}; + const b3Vector3 n=b3Cross(dl[0],dl[1]); + const b3Scalar l=n.length2(); + if(l>GJK_SIMPLEX3_EPS) + { + b3Scalar mindist=-1; + b3Scalar subw[2]={0.f,0.f}; + unsigned int subm(0); + for(unsigned int i=0;i<3;++i) + { + if(b3Dot(*vt[i],b3Cross(dl[i],n))>0) + { + const unsigned int j=imd3[i]; + const b3Scalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); + if((mindist<0)||(subd(((subm&1)?1<GJK_SIMPLEX4_EPS)) + { + b3Scalar mindist=-1; + b3Scalar subw[3]={0.f,0.f,0.f}; + unsigned int subm(0); + for(unsigned int i=0;i<3;++i) + { + const unsigned int j=imd3[i]; + const b3Scalar s=vl*b3Dot(d,b3Cross(dl[i],dl[j])); + if(s>0) + { + const b3Scalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); + if((mindist<0)||(subd((subm&1?1<e[ea]=(unsigned char)eb;fa->f[ea]=fb; + fb->e[eb]=(unsigned char)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 = b3MakeVector3(0,0,0); + m_depth = 0; + m_nextsv = 0; + for(unsigned int i=0;i1)&&gjk.EncloseOrigin()) + { + + /* Clean up */ + while(m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull,f); + append(m_stock,f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, + simplex.c[1]->w-simplex.c[3]->w, + simplex.c[2]->w-simplex.c[3]->w)<0) + { + b3Swap(simplex.c[0],simplex.c[1]); + b3Swap(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; + unsigned int pass=0; + unsigned int iterations=0; + bind(tetra[0],0,tetra[1],0); + bind(tetra[0],1,tetra[2],0); + bind(tetra[0],2,tetra[3],0); + bind(tetra[1],1,tetra[3],2); + bind(tetra[1],2,tetra[2],1); + bind(tetra[2],2,tetra[3],1); + m_status=eStatus::Valid; + for(;iterationspass = (unsigned char)(++pass); + gjk.getsupport(best->n,*w); + const b3Scalar wdist=b3Dot(best->n,w->w)-best->d; + if(wdist>EPA_ACCURACY) + { + for(unsigned int 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(); + outer=*best; + } else { + m_status=eStatus::Failed; + //m_status=eStatus::InvalidHull; + break; } + } else { m_status=eStatus::AccuraryReached;break; } + } else { m_status=eStatus::OutOfVertices;break; } + } + const b3Vector3 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] = b3Cross( outer.c[1]->w-projection, + outer.c[2]->w-projection).length(); + m_result.p[1] = b3Cross( outer.c[2]->w-projection, + outer.c[0]->w-projection).length(); + m_result.p[2] = b3Cross( outer.c[0]->w-projection, + outer.c[1]->w-projection).length(); + const b3Scalar 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 b3Scalar nl=m_normal.length(); + if(nl>0) + m_normal = m_normal/nl; + else + m_normal = b3MakeVector3(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); + } + bool getedgedist(sFace* face, sSV* a, sSV* b, b3Scalar& dist) + { + const b3Vector3 ba = b->w - a->w; + const b3Vector3 n_ab = b3Cross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const b3Scalar a_dot_nab = b3Dot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required + + if(a_dot_nab < 0) + { + // Outside of edge a->b + + const b3Scalar ba_l2 = ba.length2(); + const b3Scalar a_dot_ba = b3Dot(a->w, ba); + const b3Scalar b_dot_ba = b3Dot(b->w, ba); + + if(a_dot_ba > 0) + { + // Pick distance vertex a + dist = a->w.length(); + } + else if(b_dot_ba < 0) + { + // Pick distance vertex b + dist = b->w.length(); + } + else + { + // Pick distance to edge a->b + const b3Scalar a_dot_b = b3Dot(a->w, b->w); + dist = b3Sqrt(b3Max((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (b3Scalar)0)); + } + + return true; + } + + return false; + } + 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 = b3Cross(b->w-a->w,c->w-a->w); + const b3Scalar l=face->n.length(); + const bool v=l>EPA_ACCURACY; + + if(v) + { + if(!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) + { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = b3Dot(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; + b3Scalar mind=minf->d*minf->d; + for(sFace* f=minf->l[1];f;f=f->l[1]) + { + const b3Scalar sqd=f->d*f->d; + if(sqdpass!=pass) + { + const unsigned int e1=i1m3[e]; + if((b3Dot(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 unsigned int e2=i2m3[e]; + f->pass = (unsigned char)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 b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& verticesB, + b3GjkEpaSolver2::sResults& results, + tShape& shape, + bool withmargins) + { + /* Results */ + results.witnesses[0] = + results.witnesses[1] = b3MakeVector3(0,0,0); + results.status = b3GjkEpaSolver2::sResults::Separated; + /* Shape */ + shape.m_shapes[0] = hullA; + shape.m_shapes[1] = hullB; + shape.m_toshape1 = transB.getBasis().transposeTimes(transA.getBasis()); + shape.m_toshape0 = transA.inverseTimes(transB); + shape.EnableMargin(withmargins); + } + +} + +// +// Api +// + +using namespace gjkepa2_impl2; + +// +int b3GjkEpaSolver2::StackSizeRequirement() +{ + return(sizeof(b3GJK)+sizeof(b3EPA)); +} + +// +bool b3GjkEpaSolver2::Distance( const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& verticesB, + const b3Vector3& guess, + sResults& results) +{ + tShape shape; + Initialize(transA,transB,hullA,hullB,verticesA,verticesB,results,shape,false); + b3GJK gjk(verticesA,verticesB); + b3GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); + if(gjk_status==b3GJK::eStatus::Valid) + { + b3Vector3 w0=b3MakeVector3(0,0,0); + b3Vector3 w1=b3MakeVector3(0,0,0); + for(unsigned int i=0;irank;++i) + { + const b3Scalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0,verticesA,verticesB)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1,verticesA,verticesB)*p; + } + results.witnesses[0] = transA*w0; + results.witnesses[1] = transA*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==b3GJK::eStatus::Inside? + sResults::Penetrating : + sResults::GJK_Failed ; + return(false); + } +} + +// +bool b3GjkEpaSolver2::Penetration( const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& verticesB, + const b3Vector3& guess, + sResults& results, + bool usemargins) +{ + + tShape shape; + Initialize(transA,transB,hullA,hullB,verticesA,verticesB,results,shape,usemargins); + b3GJK gjk(verticesA,verticesB); + b3GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); + switch(gjk_status) + { + case b3GJK::eStatus::Inside: + { + b3EPA epa; + b3EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); + if(epa_status!=b3EPA::eStatus::Failed) + { + b3Vector3 w0=b3MakeVector3(0,0,0); + for(unsigned int i=0;id,0,verticesA,verticesB)*epa.m_result.p[i]; + } + results.status = sResults::Penetrating; + results.witnesses[0] = transA*w0; + results.witnesses[1] = transA*(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 b3GJK::eStatus::Failed: + results.status=sResults::GJK_Failed; + break; + default: + { + } + } + return(false); +} + + +#if 0 +// +b3Scalar b3GjkEpaSolver2::SignedDistance(const b3Vector3& position, + b3Scalar margin, + const b3Transform& transA, + const b3ConvexPolyhedronData& hullA, + const b3AlignedObjectArray& verticesA, + sResults& results) +{ + tShape shape; + btSphereShape shape1(margin); + b3Transform wtrs1(b3Quaternion(0,0,0,1),position); + Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); + GJK gjk; + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,b3Vector3(1,1,1)); + if(gjk_status==GJK::eStatus::Valid) + { + b3Vector3 w0=b3Vector3(0,0,0); + b3Vector3 w1=b3Vector3(0,0,0); + for(unsigned int i=0;irank;++i) + { + const b3Scalar 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 b3Vector3 delta= results.witnesses[1]- + results.witnesses[0]; + const b3Scalar margin= shape0->getMarginNonVirtual()+ + shape1.getMarginNonVirtual(); + const b3Scalar 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 b3Vector3 delta= results.witnesses[0]- + results.witnesses[1]; + const b3Scalar length= delta.length(); + if (length >= B3_EPSILON) + results.normal = delta/length; + return(-length); + } + } + } + return(B3_INFINITY); +} + +// +bool b3GjkEpaSolver2::SignedDistance(const btConvexShape* shape0, + const b3Transform& wtrs0, + const btConvexShape* shape1, + const b3Transform& wtrs1, + const b3Vector3& guess, + sResults& results) +{ + if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) + return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); + else + return(true); +} +#endif + + +/* Symbols cleanup */ + +#undef GJK_MAX_ITERATIONS +#undef GJK_ACCURACY +#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 diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h new file mode 100644 index 000000000000..976238a04c84 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h @@ -0,0 +1,82 @@ +/* +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 +*/ +#ifndef B3_GJK_EPA2_H +#define B3_GJK_EPA2_H + +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Transform.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" + + +///btGjkEpaSolver contributed under zlib by Nathanael Presson +struct b3GjkEpaSolver2 +{ +struct sResults + { + enum eStatus + { + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } status; + b3Vector3 witnesses[2]; + b3Vector3 normal; + b3Scalar distance; + }; + +static int StackSizeRequirement(); + +static bool Distance( const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& verticesB, + const b3Vector3& guess, + sResults& results); + +static bool Penetration( const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronData* hullA, const b3ConvexPolyhedronData* hullB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& verticesB, + const b3Vector3& guess, + sResults& results, + bool usemargins=true); +#if 0 +static b3Scalar SignedDistance( const b3Vector3& position, + b3Scalar margin, + const btConvexShape* shape, + const btTransform& wtrs, + sResults& results); + +static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + const b3Vector3& guess, + sResults& results); +#endif + +}; + +#endif //B3_GJK_EPA2_H + diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp new file mode 100644 index 000000000000..e9e51d5a36ca --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.cpp @@ -0,0 +1,390 @@ +/* +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 "b3OptimizedBvh.h" +#include "b3StridingMeshInterface.h" +#include "Bullet3Geometry/b3AabbUtil.h" + + +b3OptimizedBvh::b3OptimizedBvh() +{ +} + +b3OptimizedBvh::~b3OptimizedBvh() +{ +} + + +void b3OptimizedBvh::build(b3StridingMeshInterface* triangles, bool useQuantizedAabbCompression, const b3Vector3& bvhAabbMin, const b3Vector3& bvhAabbMax) +{ + m_useQuantization = useQuantizedAabbCompression; + + + // NodeArray triangleNodes; + + struct NodeTriangleCallback : public b3InternalTriangleIndexCallback + { + + NodeArray& m_triangleNodes; + + NodeTriangleCallback& operator=(NodeTriangleCallback& other) + { + m_triangleNodes.copyFromArray(other.m_triangleNodes); + return *this; + } + + NodeTriangleCallback(NodeArray& triangleNodes) + :m_triangleNodes(triangleNodes) + { + } + + virtual void internalProcessTriangleIndex(b3Vector3* triangle,int partId,int triangleIndex) + { + b3OptimizedBvhNode node; + b3Vector3 aabbMin,aabbMax; + aabbMin.setValue(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT)); + aabbMax.setValue(b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_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 b3InternalTriangleIndexCallback + { + QuantizedNodeArray& m_triangleNodes; + const b3QuantizedBvh* m_optimizedTree; // for quantization + + QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other) + { + m_triangleNodes.copyFromArray(other.m_triangleNodes); + m_optimizedTree = other.m_optimizedTree; + return *this; + } + + QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const b3QuantizedBvh* tree) + :m_triangleNodes(triangleNodes),m_optimizedTree(tree) + { + } + + virtual void internalProcessTriangleIndex(b3Vector3* triangle,int partId,int triangleIndex) + { + // The partId and triangle index must fit in the same (positive) integer + b3Assert(partId < (1<=0); + + b3QuantizedBvhNode node; + b3Vector3 aabbMin,aabbMax; + aabbMin.setValue(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT)); + aabbMax.setValue(b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_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 b3Scalar MIN_AABB_DIMENSION = b3Scalar(0.002); + const b3Scalar MIN_AABB_HALF_DIMENSION = b3Scalar(0.001); + if (aabbMax.getX() - aabbMin.getX() < MIN_AABB_DIMENSION) + { + aabbMax.setX(aabbMax.getX() + MIN_AABB_HALF_DIMENSION); + aabbMin.setX(aabbMin.getX() - MIN_AABB_HALF_DIMENSION); + } + if (aabbMax.getY() - aabbMin.getY() < MIN_AABB_DIMENSION) + { + aabbMax.setY(aabbMax.getY() + MIN_AABB_HALF_DIMENSION); + aabbMin.setY(aabbMin.getY() - MIN_AABB_HALF_DIMENSION); + } + if (aabbMax.getZ() - aabbMin.getZ() < MIN_AABB_DIMENSION) + { + aabbMax.setZ(aabbMax.getZ() + MIN_AABB_HALF_DIMENSION); + aabbMin.setZ(aabbMin.getZ() - 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); + + b3Vector3 aabbMin=b3MakeVector3(b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_LARGE_FLOAT)); + b3Vector3 aabbMax=b3MakeVector3(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_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()) + { + b3BvhSubtreeInfo& 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 b3OptimizedBvh::refit(b3StridingMeshInterface* meshInterface,const b3Vector3& aabbMin,const b3Vector3& 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_bvhAabbMin.getX()); + b3Assert(aabbMin.getY() > m_bvhAabbMin.getY()); + b3Assert(aabbMin.getZ() > m_bvhAabbMin.getZ()); + + b3Assert(aabbMax.getX() < m_bvhAabbMax.getX()); + b3Assert(aabbMax.getY() < m_bvhAabbMax.getY()); + b3Assert(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;im_SubtreeHeaders.size();i++) + { + b3BvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; + + //PCK: unsigned instead of bool + unsigned overlap = b3TestQuantizedAabbAgainstQuantizedAabb(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 b3OptimizedBvh::updateBvhNodes(b3StridingMeshInterface* meshInterface,int firstNode,int endNode,int index) +{ + (void)index; + + b3Assert(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; + + b3Vector3 triangleVerts[3]; + b3Vector3 aabbMin,aabbMax; + const b3Vector3& meshScaling = meshInterface->getScaling(); + + int i; + for (i=endNode-1;i>=firstNode;i--) + { + + + b3QuantizedBvhNode& 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; + b3Assert(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] = b3MakeVector3( + graphicsbase[0]*meshScaling.getX(), + graphicsbase[1]*meshScaling.getY(), + graphicsbase[2]*meshScaling.getZ()); + } + else + { + double* graphicsbase = (double*)(vertexbase+graphicsindex*stride); + triangleVerts[j] = b3MakeVector3( b3Scalar(graphicsbase[0]*meshScaling.getX()), b3Scalar(graphicsbase[1]*meshScaling.getY()), b3Scalar(graphicsbase[2]*meshScaling.getZ())); + } + } + + + + aabbMin.setValue(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT)); + aabbMax.setValue(b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_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 + + b3QuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1]; + + b3QuantizedBvhNode* 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' +b3OptimizedBvh* b3OptimizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) +{ + b3QuantizedBvh* bvh = b3QuantizedBvh::deSerializeInPlace(i_alignedDataBuffer,i_dataBufferSize,i_swapEndian); + + //we don't add additional data so just do a static upcast + return static_cast(bvh); +} diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.h new file mode 100644 index 000000000000..0272ef83bfc6 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.h @@ -0,0 +1,65 @@ +/* +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. +*/ + +///Contains contributions from Disney Studio's + +#ifndef B3_OPTIMIZED_BVH_H +#define B3_OPTIMIZED_BVH_H + +#include "b3QuantizedBvh.h" + +class b3StridingMeshInterface; + + +///The b3OptimizedBvh extends the b3QuantizedBvh to create AABB tree for triangle meshes, through the b3StridingMeshInterface. +B3_ATTRIBUTE_ALIGNED16(class) b3OptimizedBvh : public b3QuantizedBvh +{ + +public: + B3_DECLARE_ALIGNED_ALLOCATOR(); + +protected: + +public: + + b3OptimizedBvh(); + + virtual ~b3OptimizedBvh(); + + void build(b3StridingMeshInterface* triangles,bool useQuantizedAabbCompression, const b3Vector3& bvhAabbMin, const b3Vector3& bvhAabbMax); + + void refit(b3StridingMeshInterface* triangles,const b3Vector3& aabbMin,const b3Vector3& aabbMax); + + void refitPartial(b3StridingMeshInterface* triangles,const b3Vector3& aabbMin, const b3Vector3& aabbMax); + + void updateBvhNodes(b3StridingMeshInterface* meshInterface,int firstNode,int endNode,int index); + + /// Data buffer MUST be 16 byte aligned + virtual bool serializeInPlace(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const + { + return b3QuantizedBvh::serialize(o_alignedDataBuffer,i_dataBufferSize,i_swapEndian); + + } + + ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' + static b3OptimizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); + + +}; + + +#endif //B3_OPTIMIZED_BVH_H + + diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp new file mode 100644 index 000000000000..52027e111845 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp @@ -0,0 +1,1301 @@ +/* +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 "b3QuantizedBvh.h" + +#include "Bullet3Geometry/b3AabbUtil.h" + + +#define RAYAABB2 + +b3QuantizedBvh::b3QuantizedBvh() : + m_bulletVersion(B3_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(-B3_INFINITY,-B3_INFINITY,-B3_INFINITY); + m_bvhAabbMax.setValue(B3_INFINITY,B3_INFINITY,B3_INFINITY); +} + + + + + +void b3QuantizedBvh::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()) + { + b3BvhSubtreeInfo& 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 +b3Vector3 color[4]= +{ + b3Vector3(1,0,0), + b3Vector3(0,1,0), + b3Vector3(0,0,1), + b3Vector3(0,1,1) +}; +#endif //DEBUG_PATCH_COLORS + + + +void b3QuantizedBvh::setQuantizationValues(const b3Vector3& bvhAabbMin,const b3Vector3& bvhAabbMax,b3Scalar quantizationMargin) +{ + //enlarge the AABB to avoid division by zero when initializing the quantization values + b3Vector3 clampValue =b3MakeVector3(quantizationMargin,quantizationMargin,quantizationMargin); + m_bvhAabbMin = bvhAabbMin - clampValue; + m_bvhAabbMax = bvhAabbMax + clampValue; + b3Vector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin; + m_bvhQuantization = b3MakeVector3(b3Scalar(65533.0),b3Scalar(65533.0),b3Scalar(65533.0)) / aabbSize; + m_useQuantization = true; +} + + + + +b3QuantizedBvh::~b3QuantizedBvh() +{ +} + +#ifdef DEBUG_TREE_BUILDING +int gStackDepth = 0; +int gMaxStackDepth = 0; +#endif //DEBUG_TREE_BUILDING + +void b3QuantizedBvh::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; + + b3Assert(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 b3Vector3(B3_INFINITY,B3_INFINITY,B3_INFINITY)) because of quantization + setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);//can't use b3Vector3(-B3_INFINITY,-B3_INFINITY,-B3_INFINITY)) because of quantization + + + for (i=startIndex;im_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(b3QuantizedBvhNode); + const int treeSizeInBytes = escapeIndex * sizeQuantizedNode; + if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES) + { + updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex); + } + } else + { + + } + + setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex); + +} + +void b3QuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex) +{ + b3Assert(m_useQuantization); + + b3QuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex]; + int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex(); + int leftSubTreeSizeInBytes = leftSubTreeSize * static_cast(sizeof(b3QuantizedBvhNode)); + + b3QuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex]; + int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex(); + int rightSubTreeSizeInBytes = rightSubTreeSize * static_cast(sizeof(b3QuantizedBvhNode)); + + if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + { + b3BvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); + subtree.setAabbFromQuantizeNode(leftChildNode); + subtree.m_rootNodeIndex = leftChildNodexIndex; + subtree.m_subtreeSize = leftSubTreeSize; + } + + if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + { + b3BvhSubtreeInfo& 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 b3QuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis) +{ + int i; + int splitIndex =startIndex; + int numIndices = endIndex - startIndex; + b3Scalar splitValue; + + b3Vector3 means=b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + for (i=startIndex;i 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; + b3Assert(!unbal); + + return splitIndex; +} + + +int b3QuantizedBvh::calcSplittingAxis(int startIndex,int endIndex) +{ + int i; + + b3Vector3 means=b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + b3Vector3 variance=b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + int numIndices = endIndex-startIndex; + + for (i=startIndex;im_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 (b3s_maxIterations < walkIterations) + b3s_maxIterations = walkIterations; + +} + +/* +///this was the original recursive traversal, before we optimized towards stackless traversal +void b3QuantizedBvh::walkTree(b3OptimizedBvhNode* rootNode,b3NodeOverlapCallback* nodeCallback,const b3Vector3& aabbMin,const b3Vector3& 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 b3QuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const b3QuantizedBvhNode* currentNode,b3NodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +{ + b3Assert(m_useQuantization); + + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned aabbOverlap; + + //PCK: unsigned instead of bool + aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabb(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 b3QuantizedBvhNode* leftChildNode = currentNode+1; + walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + + const b3QuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode+1:leftChildNode+leftChildNode->getEscapeIndex(); + walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + } + } +} + + + +void b3QuantizedBvh::walkStacklessTreeAgainstRay(b3NodeOverlapCallback* nodeCallback, const b3Vector3& raySource, const b3Vector3& rayTarget, const b3Vector3& aabbMin, const b3Vector3& aabbMax, int startNodeIndex,int endNodeIndex) const +{ + b3Assert(!m_useQuantization); + + const b3OptimizedBvhNode* rootNode = &m_contiguousNodes[0]; + int escapeIndex, curIndex = 0; + int walkIterations = 0; + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned aabbOverlap=0; + unsigned rayBoxOverlap=0; + b3Scalar lambda_max = 1.0; + + /* Quick pruning by quantized box */ + b3Vector3 rayAabbMin = raySource; + b3Vector3 rayAabbMax = raySource; + rayAabbMin.setMin(rayTarget); + rayAabbMax.setMax(rayTarget); + + /* Add box cast extents to bounding box */ + rayAabbMin += aabbMin; + rayAabbMax += aabbMax; + +#ifdef RAYAABB2 + b3Vector3 rayDir = (rayTarget-raySource); + rayDir.normalize (); + lambda_max = rayDir.dot(rayTarget-raySource); + ///what about division by zero? --> just set rayDirection[i] to 1.0 + b3Vector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[2]; + unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; +#endif + + b3Vector3 bounds[2]; + + while (curIndex < m_curNodeIndex) + { + b3Scalar param = 1.0; + //catch bugs in tree data + b3Assert (walkIterations < m_curNodeIndex); + + walkIterations++; + + bounds[0] = rootNode->m_aabbMinOrg; + bounds[1] = rootNode->m_aabbMaxOrg; + /* Add box cast extents */ + bounds[0] -= aabbMax; + bounds[1] -= aabbMin; + + aabbOverlap = b3TestAabbAgainstAabb2(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 ? b3RayAabb2 (raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false; + +#else + b3Vector3 normal; + rayBoxOverlap = b3RayAabb(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 (b3s_maxIterations < walkIterations) + b3s_maxIterations = walkIterations; + +} + + + +void b3QuantizedBvh::walkStacklessQuantizedTreeAgainstRay(b3NodeOverlapCallback* nodeCallback, const b3Vector3& raySource, const b3Vector3& rayTarget, const b3Vector3& aabbMin, const b3Vector3& aabbMax, int startNodeIndex,int endNodeIndex) const +{ + b3Assert(m_useQuantization); + + int curIndex = startNodeIndex; + int walkIterations = 0; + int subTreeSize = endNodeIndex - startNodeIndex; + (void)subTreeSize; + + const b3QuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; + int escapeIndex; + + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned boxBoxOverlap = 0; + unsigned rayBoxOverlap = 0; + + b3Scalar lambda_max = 1.0; + +#ifdef RAYAABB2 + b3Vector3 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] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDirection[0]; + rayDirection[1] = rayDirection[1] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDirection[1]; + rayDirection[2] = rayDirection[2] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(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 */ + b3Vector3 rayAabbMin = raySource; + b3Vector3 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 b3IDebugDraw* debugDrawerPtr; + if (curIndex==drawPatch) + { + b3Vector3 aabbMin,aabbMax; + aabbMin = unQuantize(rootNode->m_quantizedAabbMin); + aabbMax = unQuantize(rootNode->m_quantizedAabbMax); + b3Vector3 color(1,0,0); + debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + } +#endif//VISUALLY_ANALYZE_BVH + + //catch bugs in tree data + b3Assert (walkIterations < subTreeSize); + + walkIterations++; + //PCK: unsigned instead of bool + // only interested if this is closer than any previous hit + b3Scalar param = 1.0; + rayBoxOverlap = 0; + boxBoxOverlap = b3TestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + isLeafNode = rootNode->isLeafNode(); + if (boxBoxOverlap) + { + b3Vector3 bounds[2]; + bounds[0] = unQuantize(rootNode->m_quantizedAabbMin); + bounds[1] = unQuantize(rootNode->m_quantizedAabbMax); + /* Add box cast extents */ + bounds[0] -= aabbMax; + bounds[1] -= aabbMin; +#if 0 + b3Vector3 normal; + bool ra2 = b3RayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); + bool ra = b3RayAabb (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 + + //B3_PROFILE("b3RayAabb2"); + rayBoxOverlap = b3RayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max); + +#else + rayBoxOverlap = true;//b3RayAabb(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 (b3s_maxIterations < walkIterations) + b3s_maxIterations = walkIterations; + +} + +void b3QuantizedBvh::walkStacklessQuantizedTree(b3NodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const +{ + b3Assert(m_useQuantization); + + int curIndex = startNodeIndex; + int walkIterations = 0; + int subTreeSize = endNodeIndex - startNodeIndex; + (void)subTreeSize; + + const b3QuantizedBvhNode* 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 b3IDebugDraw* debugDrawerPtr; + if (curIndex==drawPatch) + { + b3Vector3 aabbMin,aabbMax; + aabbMin = unQuantize(rootNode->m_quantizedAabbMin); + aabbMax = unQuantize(rootNode->m_quantizedAabbMax); + b3Vector3 color(1,0,0); + debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + } +#endif//VISUALLY_ANALYZE_BVH + + //catch bugs in tree data + b3Assert (walkIterations < subTreeSize); + + walkIterations++; + //PCK: unsigned instead of bool + aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabb(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 (b3s_maxIterations < walkIterations) + b3s_maxIterations = walkIterations; + +} + +//This traversal can be called from Playstation 3 SPU +void b3QuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(b3NodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +{ + b3Assert(m_useQuantization); + + int i; + + + for (i=0;im_SubtreeHeaders.size();i++) + { + const b3BvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; + + //PCK: unsigned instead of bool + unsigned overlap = b3TestQuantizedAabbAgainstQuantizedAabb(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 b3QuantizedBvh::reportRayOverlappingNodex (b3NodeOverlapCallback* nodeCallback, const b3Vector3& raySource, const b3Vector3& rayTarget) const +{ + reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,b3MakeVector3(0,0,0),b3MakeVector3(0,0,0)); +} + + +void b3QuantizedBvh::reportBoxCastOverlappingNodex(b3NodeOverlapCallback* nodeCallback, const b3Vector3& raySource, const b3Vector3& rayTarget, const b3Vector3& aabbMin,const b3Vector3& 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 + b3Vector3 qaabbMin = raySource; + b3Vector3 qaabbMax = raySource; + qaabbMin.setMin(rayTarget); + qaabbMax.setMax(rayTarget); + qaabbMin += aabbMin; + qaabbMax += aabbMax; + reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax); + } + */ + +} + + +void b3QuantizedBvh::swapLeafNodes(int i,int splitIndex) +{ + if (m_useQuantization) + { + b3QuantizedBvhNode tmp = m_quantizedLeafNodes[i]; + m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex]; + m_quantizedLeafNodes[splitIndex] = tmp; + } else + { + b3OptimizedBvhNode tmp = m_leafNodes[i]; + m_leafNodes[i] = m_leafNodes[splitIndex]; + m_leafNodes[splitIndex] = tmp; + } +} + +void b3QuantizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex) +{ + if (m_useQuantization) + { + m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex]; + } else + { + m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex]; + } +} + +//PCK: include +#include + +#if 0 +//PCK: consts +static const unsigned BVH_ALIGNMENT = 16; +static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1; + +static const unsigned BVH_ALIGNMENT_BLOCKS = 2; +#endif + + +unsigned int b3QuantizedBvh::getAlignmentSerializationPadding() +{ + // I changed this to 0 since the extra padding is not needed or used. + return 0;//BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; +} + +unsigned b3QuantizedBvh::calculateSerializeBufferSize() const +{ + unsigned baseSize = sizeof(b3QuantizedBvh) + getAlignmentSerializationPadding(); + baseSize += sizeof(b3BvhSubtreeInfo) * m_subtreeHeaderCount; + if (m_useQuantization) + { + return baseSize + m_curNodeIndex * sizeof(b3QuantizedBvhNode); + } + return baseSize + m_curNodeIndex * sizeof(b3OptimizedBvhNode); +} + +bool b3QuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) const +{ + b3Assert(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? + b3Assert(0); + return false; + } +*/ + + b3QuantizedBvh *targetBvh = (b3QuantizedBvh *)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) b3QuantizedBvh; + + if (i_swapEndian) + { + targetBvh->m_curNodeIndex = static_cast(b3SwapEndian(m_curNodeIndex)); + + + b3SwapVector3Endian(m_bvhAabbMin,targetBvh->m_bvhAabbMin); + b3SwapVector3Endian(m_bvhAabbMax,targetBvh->m_bvhAabbMax); + b3SwapVector3Endian(m_bvhQuantization,targetBvh->m_bvhQuantization); + + targetBvh->m_traversalMode = (b3TraversalMode)b3SwapEndian(m_traversalMode); + targetBvh->m_subtreeHeaderCount = static_cast(b3SwapEndian(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(b3QuantizedBvh); + + 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] = b3SwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = b3SwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = b3SwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]); + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = b3SwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = b3SwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = b3SwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]); + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast(b3SwapEndian(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(b3QuantizedBvhNode) * 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++) + { + b3SwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMinOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg); + b3SwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMaxOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg); + + targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast(b3SwapEndian(m_contiguousNodes[nodeIndex].m_escapeIndex)); + targetBvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast(b3SwapEndian(m_contiguousNodes[nodeIndex].m_subPart)); + targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast(b3SwapEndian(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(b3OptimizedBvhNode) * 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] = b3SwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = b3SwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = b3SwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[2]); + + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = b3SwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = b3SwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = b3SwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[2]); + + targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast(b3SwapEndian(m_SubtreeHeaders[i].m_rootNodeIndex)); + targetBvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast(b3SwapEndian(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(b3BvhSubtreeInfo) * 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; +} + +b3QuantizedBvh *b3QuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) +{ + + if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) + { + return NULL; + } + b3QuantizedBvh *bvh = (b3QuantizedBvh *)i_alignedDataBuffer; + + if (i_swapEndian) + { + bvh->m_curNodeIndex = static_cast(b3SwapEndian(bvh->m_curNodeIndex)); + + b3UnSwapVector3Endian(bvh->m_bvhAabbMin); + b3UnSwapVector3Endian(bvh->m_bvhAabbMax); + b3UnSwapVector3Endian(bvh->m_bvhQuantization); + + bvh->m_traversalMode = (b3TraversalMode)b3SwapEndian(bvh->m_traversalMode); + bvh->m_subtreeHeaderCount = static_cast(b3SwapEndian(bvh->m_subtreeHeaderCount)); + } + + unsigned int calculatedBufSize = bvh->calculateSerializeBufferSize(); + b3Assert(calculatedBufSize <= i_dataBufferSize); + + if (calculatedBufSize > i_dataBufferSize) + { + return NULL; + } + + unsigned char *nodeData = (unsigned char *)bvh; + nodeData += sizeof(b3QuantizedBvh); + + 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) b3QuantizedBvh(*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] = b3SwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = b3SwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = b3SwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]); + + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = b3SwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = b3SwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = b3SwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]); + + bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast(b3SwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex)); + } + } + nodeData += sizeof(b3QuantizedBvhNode) * nodeCount; + } + else + { + bvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); + + if (i_swapEndian) + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + b3UnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg); + b3UnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg); + + bvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast(b3SwapEndian(bvh->m_contiguousNodes[nodeIndex].m_escapeIndex)); + bvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast(b3SwapEndian(bvh->m_contiguousNodes[nodeIndex].m_subPart)); + bvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast(b3SwapEndian(bvh->m_contiguousNodes[nodeIndex].m_triangleIndex)); + } + } + nodeData += sizeof(b3OptimizedBvhNode) * 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] = b3SwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = b3SwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = b3SwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2]); + + bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = b3SwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = b3SwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = b3SwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2]); + + bvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast(b3SwapEndian(bvh->m_SubtreeHeaders[i].m_rootNodeIndex)); + bvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast(b3SwapEndian(bvh->m_SubtreeHeaders[i].m_subtreeSize)); + } + } + + return bvh; +} + +// Constructor that prevents b3Vector3's default constructor from being called +b3QuantizedBvh::b3QuantizedBvh(b3QuantizedBvh &self, bool /* ownsMemory */) : +m_bvhAabbMin(self.m_bvhAabbMin), +m_bvhAabbMax(self.m_bvhAabbMax), +m_bvhQuantization(self.m_bvhQuantization), +m_bulletVersion(B3_BULLET_VERSION) +{ + +} + +void b3QuantizedBvh::deSerializeFloat(struct b3QuantizedBvhFloatData& quantizedBvhFloatData) +{ + m_bvhAabbMax.deSerializeFloat(quantizedBvhFloatData.m_bvhAabbMax); + m_bvhAabbMin.deSerializeFloat(quantizedBvhFloatData.m_bvhAabbMin); + m_bvhQuantization.deSerializeFloat(quantizedBvhFloatData.m_bvhQuantization); + + m_curNodeIndex = quantizedBvhFloatData.m_curNodeIndex; + m_useQuantization = quantizedBvhFloatData.m_useQuantization!=0; + + { + int numElem = quantizedBvhFloatData.m_numContiguousLeafNodes; + m_contiguousNodes.resize(numElem); + + if (numElem) + { + b3OptimizedBvhNodeFloatData* memPtr = quantizedBvhFloatData.m_contiguousNodesPtr; + + for (int i=0;im_aabbMaxOrg); + m_contiguousNodes[i].m_aabbMinOrg.deSerializeFloat(memPtr->m_aabbMinOrg); + m_contiguousNodes[i].m_escapeIndex = memPtr->m_escapeIndex; + m_contiguousNodes[i].m_subPart = memPtr->m_subPart; + m_contiguousNodes[i].m_triangleIndex = memPtr->m_triangleIndex; + } + } + } + + { + int numElem = quantizedBvhFloatData.m_numQuantizedContiguousNodes; + m_quantizedContiguousNodes.resize(numElem); + + if (numElem) + { + b3QuantizedBvhNodeData* memPtr = quantizedBvhFloatData.m_quantizedContiguousNodesPtr; + for (int i=0;im_escapeIndexOrTriangleIndex; + m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0]; + m_quantizedContiguousNodes[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1]; + m_quantizedContiguousNodes[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2]; + m_quantizedContiguousNodes[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0]; + m_quantizedContiguousNodes[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1]; + m_quantizedContiguousNodes[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2]; + } + } + } + + m_traversalMode = b3TraversalMode(quantizedBvhFloatData.m_traversalMode); + + { + int numElem = quantizedBvhFloatData.m_numSubtreeHeaders; + m_SubtreeHeaders.resize(numElem); + if (numElem) + { + b3BvhSubtreeInfoData* memPtr = quantizedBvhFloatData.m_subTreeInfoPtr; + for (int i=0;im_quantizedAabbMax[0] ; + m_SubtreeHeaders[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1]; + m_SubtreeHeaders[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2]; + m_SubtreeHeaders[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0]; + m_SubtreeHeaders[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1]; + m_SubtreeHeaders[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2]; + m_SubtreeHeaders[i].m_rootNodeIndex = memPtr->m_rootNodeIndex; + m_SubtreeHeaders[i].m_subtreeSize = memPtr->m_subtreeSize; + } + } + } +} + +void b3QuantizedBvh::deSerializeDouble(struct b3QuantizedBvhDoubleData& quantizedBvhDoubleData) +{ + m_bvhAabbMax.deSerializeDouble(quantizedBvhDoubleData.m_bvhAabbMax); + m_bvhAabbMin.deSerializeDouble(quantizedBvhDoubleData.m_bvhAabbMin); + m_bvhQuantization.deSerializeDouble(quantizedBvhDoubleData.m_bvhQuantization); + + m_curNodeIndex = quantizedBvhDoubleData.m_curNodeIndex; + m_useQuantization = quantizedBvhDoubleData.m_useQuantization!=0; + + { + int numElem = quantizedBvhDoubleData.m_numContiguousLeafNodes; + m_contiguousNodes.resize(numElem); + + if (numElem) + { + b3OptimizedBvhNodeDoubleData* memPtr = quantizedBvhDoubleData.m_contiguousNodesPtr; + + for (int i=0;im_aabbMaxOrg); + m_contiguousNodes[i].m_aabbMinOrg.deSerializeDouble(memPtr->m_aabbMinOrg); + m_contiguousNodes[i].m_escapeIndex = memPtr->m_escapeIndex; + m_contiguousNodes[i].m_subPart = memPtr->m_subPart; + m_contiguousNodes[i].m_triangleIndex = memPtr->m_triangleIndex; + } + } + } + + { + int numElem = quantizedBvhDoubleData.m_numQuantizedContiguousNodes; + m_quantizedContiguousNodes.resize(numElem); + + if (numElem) + { + b3QuantizedBvhNodeData* memPtr = quantizedBvhDoubleData.m_quantizedContiguousNodesPtr; + for (int i=0;im_escapeIndexOrTriangleIndex; + m_quantizedContiguousNodes[i].m_quantizedAabbMax[0] = memPtr->m_quantizedAabbMax[0]; + m_quantizedContiguousNodes[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1]; + m_quantizedContiguousNodes[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2]; + m_quantizedContiguousNodes[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0]; + m_quantizedContiguousNodes[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1]; + m_quantizedContiguousNodes[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2]; + } + } + } + + m_traversalMode = b3TraversalMode(quantizedBvhDoubleData.m_traversalMode); + + { + int numElem = quantizedBvhDoubleData.m_numSubtreeHeaders; + m_SubtreeHeaders.resize(numElem); + if (numElem) + { + b3BvhSubtreeInfoData* memPtr = quantizedBvhDoubleData.m_subTreeInfoPtr; + for (int i=0;im_quantizedAabbMax[0] ; + m_SubtreeHeaders[i].m_quantizedAabbMax[1] = memPtr->m_quantizedAabbMax[1]; + m_SubtreeHeaders[i].m_quantizedAabbMax[2] = memPtr->m_quantizedAabbMax[2]; + m_SubtreeHeaders[i].m_quantizedAabbMin[0] = memPtr->m_quantizedAabbMin[0]; + m_SubtreeHeaders[i].m_quantizedAabbMin[1] = memPtr->m_quantizedAabbMin[1]; + m_SubtreeHeaders[i].m_quantizedAabbMin[2] = memPtr->m_quantizedAabbMin[2]; + m_SubtreeHeaders[i].m_rootNodeIndex = memPtr->m_rootNodeIndex; + m_SubtreeHeaders[i].m_subtreeSize = memPtr->m_subtreeSize; + } + } + } + +} + + + +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* b3QuantizedBvh::serialize(void* dataBuffer, b3Serializer* serializer) const +{ + b3Assert(0); + return 0; +} + + + + + diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h new file mode 100644 index 000000000000..63c523c7584d --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h @@ -0,0 +1,556 @@ +/* +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 B3_QUANTIZED_BVH_H +#define B3_QUANTIZED_BVH_H + +class b3Serializer; + +//#define DEBUG_CHECK_DEQUANTIZATION 1 +#ifdef DEBUG_CHECK_DEQUANTIZATION +#ifdef __SPU__ +#define printf spu_printf +#endif //__SPU__ + +#include +#include +#endif //DEBUG_CHECK_DEQUANTIZATION + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3AlignedAllocator.h" + +#ifdef B3_USE_DOUBLE_PRECISION +#define b3QuantizedBvhData b3QuantizedBvhDoubleData +#define b3OptimizedBvhNodeData b3OptimizedBvhNodeDoubleData +#define b3QuantizedBvhDataName "b3QuantizedBvhDoubleData" +#else +#define b3QuantizedBvhData b3QuantizedBvhFloatData +#define b3OptimizedBvhNodeData b3OptimizedBvhNodeFloatData +#define b3QuantizedBvhDataName "b3QuantizedBvhFloatData" +#endif + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h" + + + +//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp + + +//Note: currently we have 16 bytes per quantized node +#define MAX_SUBTREE_SIZE_IN_BYTES 2048 + +// 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one +// actually) triangles each (since the sign bit is reserved +#define MAX_NUM_PARTS_IN_BITS 10 + +///b3QuantizedBvhNode is a compressed aabb node, 16 bytes. +///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). +B3_ATTRIBUTE_ALIGNED16 (struct) b3QuantizedBvhNode : public b3QuantizedBvhNodeData +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); + + bool isLeafNode() const + { + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (m_escapeIndexOrTriangleIndex >= 0); + } + int getEscapeIndex() const + { + b3Assert(!isLeafNode()); + return -m_escapeIndexOrTriangleIndex; + } + int getTriangleIndex() const + { + b3Assert(isLeafNode()); + unsigned int x=0; + unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS); + // Get only the lower bits where the triangle index is stored + return (m_escapeIndexOrTriangleIndex&~(y)); + } + int getPartId() const + { + b3Assert(isLeafNode()); + // Get only the highest bits where the part index is stored + return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); + } +} +; + +/// b3OptimizedBvhNode contains both internal and leaf node information. +/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes. +B3_ATTRIBUTE_ALIGNED16 (struct) b3OptimizedBvhNode +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); + + //32 bytes + b3Vector3 m_aabbMinOrg; + b3Vector3 m_aabbMaxOrg; + + //4 + int m_escapeIndex; + + //8 + //for child nodes + int m_subPart; + int m_triangleIndex; + +//pad the size to 64 bytes + char m_padding[20]; +}; + + +///b3BvhSubtreeInfo provides info to gather a subtree of limited size +B3_ATTRIBUTE_ALIGNED16(class) b3BvhSubtreeInfo : public b3BvhSubtreeInfoData +{ +public: + B3_DECLARE_ALIGNED_ALLOCATOR(); + + b3BvhSubtreeInfo() + { + //memset(&m_padding[0], 0, sizeof(m_padding)); + } + + + void setAabbFromQuantizeNode(const b3QuantizedBvhNode& quantizedNode) + { + m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0]; + m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1]; + m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2]; + m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0]; + m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1]; + m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2]; + } +} +; + + +class b3NodeOverlapCallback +{ +public: + virtual ~b3NodeOverlapCallback() {}; + + virtual void processNode(int subPart, int triangleIndex) = 0; +}; + +#include "Bullet3Common/b3AlignedAllocator.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + + + +///for code readability: +typedef b3AlignedObjectArray NodeArray; +typedef b3AlignedObjectArray QuantizedNodeArray; +typedef b3AlignedObjectArray BvhSubtreeInfoArray; + + +///The b3QuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU. +///It is used by the b3BvhTriangleMeshShape as midphase +///It is recommended to use quantization for better performance and lower memory requirements. +B3_ATTRIBUTE_ALIGNED16(class) b3QuantizedBvh +{ +public: + enum b3TraversalMode + { + TRAVERSAL_STACKLESS = 0, + TRAVERSAL_STACKLESS_CACHE_FRIENDLY, + TRAVERSAL_RECURSIVE + }; + + + + + b3Vector3 m_bvhAabbMin; + b3Vector3 m_bvhAabbMax; + b3Vector3 m_bvhQuantization; + +protected: + int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess. + + int m_curNodeIndex; + //quantization data + bool m_useQuantization; + + + + NodeArray m_leafNodes; + NodeArray m_contiguousNodes; + QuantizedNodeArray m_quantizedLeafNodes; + QuantizedNodeArray m_quantizedContiguousNodes; + + b3TraversalMode m_traversalMode; + BvhSubtreeInfoArray m_SubtreeHeaders; + + //This is only used for serialization so we don't have to add serialization directly to b3AlignedObjectArray + mutable int m_subtreeHeaderCount; + + + + + + ///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!) + ///this might be refactored into a virtual, it is usually not calculated at run-time + void setInternalNodeAabbMin(int nodeIndex, const b3Vector3& aabbMin) + { + if (m_useQuantization) + { + quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0); + } else + { + m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin; + + } + } + void setInternalNodeAabbMax(int nodeIndex,const b3Vector3& aabbMax) + { + if (m_useQuantization) + { + quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1); + } else + { + m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax; + } + } + + b3Vector3 getAabbMin(int nodeIndex) const + { + if (m_useQuantization) + { + return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]); + } + //non-quantized + return m_leafNodes[nodeIndex].m_aabbMinOrg; + + } + b3Vector3 getAabbMax(int nodeIndex) const + { + if (m_useQuantization) + { + return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]); + } + //non-quantized + return m_leafNodes[nodeIndex].m_aabbMaxOrg; + + } + + + void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) + { + if (m_useQuantization) + { + m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex; + } + else + { + m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex; + } + + } + + void mergeInternalNodeAabb(int nodeIndex,const b3Vector3& newAabbMin,const b3Vector3& newAabbMax) + { + if (m_useQuantization) + { + unsigned short int quantizedAabbMin[3]; + unsigned short int quantizedAabbMax[3]; + quantize(quantizedAabbMin,newAabbMin,0); + quantize(quantizedAabbMax,newAabbMax,1); + for (int i=0;i<3;i++) + { + if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i]) + m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i]; + + if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i]) + m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i]; + + } + } else + { + //non-quantized + m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin); + m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax); + } + } + + void swapLeafNodes(int firstIndex,int secondIndex); + + void assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex); + +protected: + + + + void buildTree (int startIndex,int endIndex); + + int calcSplittingAxis(int startIndex,int endIndex); + + int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis); + + void walkStacklessTree(b3NodeOverlapCallback* nodeCallback,const b3Vector3& aabbMin,const b3Vector3& aabbMax) const; + + void walkStacklessQuantizedTreeAgainstRay(b3NodeOverlapCallback* nodeCallback, const b3Vector3& raySource, const b3Vector3& rayTarget, const b3Vector3& aabbMin, const b3Vector3& aabbMax, int startNodeIndex,int endNodeIndex) const; + void walkStacklessQuantizedTree(b3NodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const; + void walkStacklessTreeAgainstRay(b3NodeOverlapCallback* nodeCallback, const b3Vector3& raySource, const b3Vector3& rayTarget, const b3Vector3& aabbMin, const b3Vector3& aabbMax, int startNodeIndex,int endNodeIndex) const; + + ///tree traversal designed for small-memory processors like PS3 SPU + void walkStacklessQuantizedTreeCacheFriendly(b3NodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; + + ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal + void walkRecursiveQuantizedTreeAgainstQueryAabb(const b3QuantizedBvhNode* currentNode,b3NodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; + + ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal + void walkRecursiveQuantizedTreeAgainstQuantizedTree(const b3QuantizedBvhNode* treeNodeA,const b3QuantizedBvhNode* treeNodeB,b3NodeOverlapCallback* nodeCallback) const; + + + + + void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); + +public: + + B3_DECLARE_ALIGNED_ALLOCATOR(); + + b3QuantizedBvh(); + + virtual ~b3QuantizedBvh(); + + + ///***************************************** expert/internal use only ************************* + void setQuantizationValues(const b3Vector3& bvhAabbMin,const b3Vector3& bvhAabbMax,b3Scalar quantizationMargin=b3Scalar(1.0)); + QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; } + ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized + void buildInternal(); + ///***************************************** expert/internal use only ************************* + + void reportAabbOverlappingNodex(b3NodeOverlapCallback* nodeCallback,const b3Vector3& aabbMin,const b3Vector3& aabbMax) const; + void reportRayOverlappingNodex (b3NodeOverlapCallback* nodeCallback, const b3Vector3& raySource, const b3Vector3& rayTarget) const; + void reportBoxCastOverlappingNodex(b3NodeOverlapCallback* nodeCallback, const b3Vector3& raySource, const b3Vector3& rayTarget, const b3Vector3& aabbMin,const b3Vector3& aabbMax) const; + + B3_FORCE_INLINE void quantize(unsigned short* out, const b3Vector3& point,int isMax) const + { + + b3Assert(m_useQuantization); + + b3Assert(point.getX() <= m_bvhAabbMax.getX()); + b3Assert(point.getY() <= m_bvhAabbMax.getY()); + b3Assert(point.getZ() <= m_bvhAabbMax.getZ()); + + b3Assert(point.getX() >= m_bvhAabbMin.getX()); + b3Assert(point.getY() >= m_bvhAabbMin.getY()); + b3Assert(point.getZ() >= m_bvhAabbMin.getZ()); + + b3Vector3 v = (point - m_bvhAabbMin) * m_bvhQuantization; + ///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative + ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly) + ///@todo: double-check this + if (isMax) + { + out[0] = (unsigned short) (((unsigned short)(v.getX()+b3Scalar(1.)) | 1)); + out[1] = (unsigned short) (((unsigned short)(v.getY()+b3Scalar(1.)) | 1)); + out[2] = (unsigned short) (((unsigned short)(v.getZ()+b3Scalar(1.)) | 1)); + } else + { + out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe)); + out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe)); + out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe)); + } + + +#ifdef DEBUG_CHECK_DEQUANTIZATION + b3Vector3 newPoint = unQuantize(out); + if (isMax) + { + if (newPoint.getX() < point.getX()) + { + printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX()); + } + if (newPoint.getY() < point.getY()) + { + printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY()); + } + if (newPoint.getZ() < point.getZ()) + { + + printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ()); + } + } else + { + if (newPoint.getX() > point.getX()) + { + printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX()); + } + if (newPoint.getY() > point.getY()) + { + printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY()); + } + if (newPoint.getZ() > point.getZ()) + { + printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ()); + } + } +#endif //DEBUG_CHECK_DEQUANTIZATION + + } + + + B3_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const b3Vector3& point2,int isMax) const + { + + b3Assert(m_useQuantization); + + b3Vector3 clampedPoint(point2); + clampedPoint.setMax(m_bvhAabbMin); + clampedPoint.setMin(m_bvhAabbMax); + + quantize(out,clampedPoint,isMax); + + } + + B3_FORCE_INLINE b3Vector3 unQuantize(const unsigned short* vecIn) const + { + b3Vector3 vecOut; + vecOut.setValue( + (b3Scalar)(vecIn[0]) / (m_bvhQuantization.getX()), + (b3Scalar)(vecIn[1]) / (m_bvhQuantization.getY()), + (b3Scalar)(vecIn[2]) / (m_bvhQuantization.getZ())); + vecOut += m_bvhAabbMin; + return vecOut; + } + + ///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees. + void setTraversalMode(b3TraversalMode traversalMode) + { + m_traversalMode = traversalMode; + } + + + B3_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray() + { + return m_quantizedContiguousNodes; + } + + + B3_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray() + { + return m_SubtreeHeaders; + } + +//////////////////////////////////////////////////////////////////// + + /////Calculate space needed to store BVH for serialization + unsigned calculateSerializeBufferSize() const; + + /// Data buffer MUST be 16 byte aligned + virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const; + + ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' + static b3QuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); + + static unsigned int getAlignmentSerializationPadding(); +////////////////////////////////////////////////////////////////////// + + + virtual int calculateSerializeBufferSizeNew() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const; + + virtual void deSerializeFloat(struct b3QuantizedBvhFloatData& quantizedBvhFloatData); + + virtual void deSerializeDouble(struct b3QuantizedBvhDoubleData& quantizedBvhDoubleData); + + +//////////////////////////////////////////////////////////////////// + + B3_FORCE_INLINE bool isQuantized() + { + return m_useQuantization; + } + +private: + // Special "copy" constructor that allows for in-place deserialization + // Prevents b3Vector3's default constructor from being called, but doesn't inialize much else + // ownsMemory should most likely be false if deserializing, and if you are not, don't call this (it also changes the function signature, which we need) + b3QuantizedBvh(b3QuantizedBvh &other, bool ownsMemory); + +} +; + + +struct b3OptimizedBvhNodeFloatData +{ + b3Vector3FloatData m_aabbMinOrg; + b3Vector3FloatData m_aabbMaxOrg; + int m_escapeIndex; + int m_subPart; + int m_triangleIndex; + char m_pad[4]; +}; + +struct b3OptimizedBvhNodeDoubleData +{ + b3Vector3DoubleData m_aabbMinOrg; + b3Vector3DoubleData m_aabbMaxOrg; + int m_escapeIndex; + int m_subPart; + int m_triangleIndex; + char m_pad[4]; +}; + + + +struct b3QuantizedBvhFloatData +{ + b3Vector3FloatData m_bvhAabbMin; + b3Vector3FloatData m_bvhAabbMax; + b3Vector3FloatData m_bvhQuantization; + int m_curNodeIndex; + int m_useQuantization; + int m_numContiguousLeafNodes; + int m_numQuantizedContiguousNodes; + b3OptimizedBvhNodeFloatData *m_contiguousNodesPtr; + b3QuantizedBvhNodeData *m_quantizedContiguousNodesPtr; + b3BvhSubtreeInfoData *m_subTreeInfoPtr; + int m_traversalMode; + int m_numSubtreeHeaders; + +}; + +struct b3QuantizedBvhDoubleData +{ + b3Vector3DoubleData m_bvhAabbMin; + b3Vector3DoubleData m_bvhAabbMax; + b3Vector3DoubleData m_bvhQuantization; + int m_curNodeIndex; + int m_useQuantization; + int m_numContiguousLeafNodes; + int m_numQuantizedContiguousNodes; + b3OptimizedBvhNodeDoubleData *m_contiguousNodesPtr; + b3QuantizedBvhNodeData *m_quantizedContiguousNodesPtr; + + int m_traversalMode; + int m_numSubtreeHeaders; + b3BvhSubtreeInfoData *m_subTreeInfoPtr; +}; + + +B3_FORCE_INLINE int b3QuantizedBvh::calculateSerializeBufferSizeNew() const +{ + return sizeof(b3QuantizedBvhData); +} + + + +#endif //B3_QUANTIZED_BVH_H diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.cpp b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.cpp new file mode 100644 index 000000000000..4d97f7f62b90 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.cpp @@ -0,0 +1,214 @@ +/* +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 "b3StridingMeshInterface.h" + + +b3StridingMeshInterface::~b3StridingMeshInterface() +{ + +} + + +void b3StridingMeshInterface::InternalProcessAllTriangles(b3InternalTriangleIndexCallback* callback,const b3Vector3& aabbMin,const b3Vector3& aabbMax) const +{ + (void)aabbMin; + (void)aabbMax; + int numtotalphysicsverts = 0; + int part,graphicssubparts = getNumSubParts(); + const unsigned char * vertexbase; + const unsigned char * indexbase; + int indexstride; + PHY_ScalarType type; + PHY_ScalarType gfxindextype; + int stride,numverts,numtriangles; + int gfxindex; + b3Vector3 triangle[3]; + + b3Vector3 meshScaling = getScaling(); + + ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype + for (part=0;partinternalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + case PHY_SHORT: + { + for (gfxindex=0;gfxindexinternalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + case PHY_UCHAR: + { + for (gfxindex=0;gfxindexinternalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + default: + b3Assert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); + } + break; + } + + case PHY_DOUBLE: + { + double* graphicsbase; + + switch (gfxindextype) + { + case PHY_INTEGER: + { + for (gfxindex=0;gfxindexinternalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + case PHY_SHORT: + { + for (gfxindex=0;gfxindexinternalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + case PHY_UCHAR: + { + for (gfxindex=0;gfxindexinternalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + default: + b3Assert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); + } + break; + } + default: + b3Assert((type == PHY_FLOAT) || (type == PHY_DOUBLE)); + } + + unLockReadOnlyVertexBase(part); + } +} + +void b3StridingMeshInterface::calculateAabbBruteForce(b3Vector3& aabbMin,b3Vector3& aabbMax) +{ + + struct AabbCalculationCallback : public b3InternalTriangleIndexCallback + { + b3Vector3 m_aabbMin; + b3Vector3 m_aabbMax; + + AabbCalculationCallback() + { + m_aabbMin.setValue(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT)); + m_aabbMax.setValue(b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_LARGE_FLOAT)); + } + + virtual void internalProcessTriangleIndex(b3Vector3* triangle,int partId,int triangleIndex) + { + (void)partId; + (void)triangleIndex; + + m_aabbMin.setMin(triangle[0]); + m_aabbMax.setMax(triangle[0]); + m_aabbMin.setMin(triangle[1]); + m_aabbMax.setMax(triangle[1]); + m_aabbMin.setMin(triangle[2]); + m_aabbMax.setMax(triangle[2]); + } + }; + + //first calculate the total aabb for all triangles + AabbCalculationCallback aabbCallback; + aabbMin.setValue(b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_LARGE_FLOAT),b3Scalar(-B3_LARGE_FLOAT)); + aabbMax.setValue(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT)); + InternalProcessAllTriangles(&aabbCallback,aabbMin,aabbMax); + + aabbMin = aabbCallback.m_aabbMin; + aabbMax = aabbCallback.m_aabbMax; +} + + diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.h new file mode 100644 index 000000000000..9513f68f77ea --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3StridingMeshInterface.h @@ -0,0 +1,167 @@ +/* +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. +*/ + +#ifndef B3_STRIDING_MESHINTERFACE_H +#define B3_STRIDING_MESHINTERFACE_H + +#include "Bullet3Common/b3Vector3.h" +#include "b3TriangleCallback.h" +//#include "b3ConcaveShape.h" + + +enum PHY_ScalarType { + PHY_FLOAT, PHY_DOUBLE, PHY_INTEGER, PHY_SHORT, + PHY_FIXEDPOINT88, PHY_UCHAR +}; + + +/// The b3StridingMeshInterface is the interface class for high performance generic access to triangle meshes, used in combination with b3BvhTriangleMeshShape and some other collision shapes. +/// Using index striding of 3*sizeof(integer) it can use triangle arrays, using index striding of 1*sizeof(integer) it can handle triangle strips. +/// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory. +B3_ATTRIBUTE_ALIGNED16(class ) b3StridingMeshInterface +{ + protected: + + b3Vector3 m_scaling; + + public: + B3_DECLARE_ALIGNED_ALLOCATOR(); + + b3StridingMeshInterface() :m_scaling(b3MakeVector3(b3Scalar(1.),b3Scalar(1.),b3Scalar(1.))) + { + + } + + virtual ~b3StridingMeshInterface(); + + + + virtual void InternalProcessAllTriangles(b3InternalTriangleIndexCallback* callback,const b3Vector3& aabbMin,const b3Vector3& aabbMax) const; + + ///brute force method to calculate aabb + void calculateAabbBruteForce(b3Vector3& aabbMin,b3Vector3& aabbMax); + + /// get read and write access to a subpart of a triangle mesh + /// this subpart has a continuous array of vertices and indices + /// in this way the mesh can be handled as chunks of memory with striding + /// very similar to OpenGL vertexarray support + /// make a call to unLockVertexBase when the read and write access is finished + virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0)=0; + + virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const=0; + + /// unLockVertexBase finishes the access to a subpart of the triangle mesh + /// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished + virtual void unLockVertexBase(int subpart)=0; + + virtual void unLockReadOnlyVertexBase(int subpart) const=0; + + + /// getNumSubParts returns the number of seperate subparts + /// each subpart has a continuous array of vertices and indices + virtual int getNumSubParts() const=0; + + virtual void preallocateVertices(int numverts)=0; + virtual void preallocateIndices(int numindices)=0; + + virtual bool hasPremadeAabb() const { return false; } + virtual void setPremadeAabb(const b3Vector3& aabbMin, const b3Vector3& aabbMax ) const + { + (void) aabbMin; + (void) aabbMax; + } + virtual void getPremadeAabb(b3Vector3* aabbMin, b3Vector3* aabbMax ) const + { + (void) aabbMin; + (void) aabbMax; + } + + const b3Vector3& getScaling() const { + return m_scaling; + } + void setScaling(const b3Vector3& scaling) + { + m_scaling = scaling; + } + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + //virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const; + + +}; + +struct b3IntIndexData +{ + int m_value; +}; + +struct b3ShortIntIndexData +{ + short m_value; + char m_pad[2]; +}; + +struct b3ShortIntIndexTripletData +{ + short m_values[3]; + char m_pad[2]; +}; + +struct b3CharIndexTripletData +{ + unsigned char m_values[3]; + char m_pad; +}; + + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct b3MeshPartData +{ + b3Vector3FloatData *m_vertices3f; + b3Vector3DoubleData *m_vertices3d; + + b3IntIndexData *m_indices32; + b3ShortIntIndexTripletData *m_3indices16; + b3CharIndexTripletData *m_3indices8; + + b3ShortIntIndexData *m_indices16;//backwards compatibility + + int m_numTriangles;//length of m_indices = m_numTriangles + int m_numVertices; +}; + + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct b3StridingMeshInterfaceData +{ + b3MeshPartData *m_meshPartsPtr; + b3Vector3FloatData m_scaling; + int m_numMeshParts; + char m_padding[4]; +}; + + + + +B3_FORCE_INLINE int b3StridingMeshInterface::calculateSerializeBufferSize() const +{ + return sizeof(b3StridingMeshInterfaceData); +} + + + +#endif //B3_STRIDING_MESHINTERFACE_H diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h new file mode 100644 index 000000000000..d073ee57c378 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h @@ -0,0 +1,38 @@ + +#ifndef B3_SUPPORT_MAPPINGS_H +#define B3_SUPPORT_MAPPINGS_H + +#include "Bullet3Common/b3Transform.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "b3VectorFloat4.h" + + +struct b3GjkPairDetector; + + + +inline b3Vector3 localGetSupportVertexWithMargin(const float4& supportVec,const struct b3ConvexPolyhedronData* hull, + const b3AlignedObjectArray& verticesA, b3Scalar margin) +{ + b3Vector3 supVec = b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + b3Scalar maxDot = b3Scalar(-B3_LARGE_FLOAT); + + // Here we take advantage of dot(a, b*c) = dot(a*b, c). Note: This is true mathematically, but not numerically. + if( 0 < hull->m_numVertices ) + { + const b3Vector3 scaled = supportVec; + int index = (int) scaled.maxDot( &verticesA[hull->m_vertexOffset], hull->m_numVertices, maxDot); + return verticesA[hull->m_vertexOffset+index]; + } + + return supVec; + +} + +inline b3Vector3 localGetSupportVertexWithoutMargin(const float4& supportVec,const struct b3ConvexPolyhedronData* hull, + const b3AlignedObjectArray& verticesA) +{ + return localGetSupportVertexWithMargin(supportVec,hull,verticesA,0.f); +} + +#endif //B3_SUPPORT_MAPPINGS_H diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.cpp b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.cpp new file mode 100644 index 000000000000..9066451884ef --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.cpp @@ -0,0 +1,28 @@ +/* +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 "b3TriangleCallback.h" + +b3TriangleCallback::~b3TriangleCallback() +{ + +} + + +b3InternalTriangleIndexCallback::~b3InternalTriangleIndexCallback() +{ + +} + diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.h new file mode 100644 index 000000000000..3059fa4f2124 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleCallback.h @@ -0,0 +1,42 @@ +/* +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. +*/ + +#ifndef B3_TRIANGLE_CALLBACK_H +#define B3_TRIANGLE_CALLBACK_H + +#include "Bullet3Common/b3Vector3.h" + + +///The b3TriangleCallback provides a callback for each overlapping triangle when calling processAllTriangles. +///This callback is called by processAllTriangles for all b3ConcaveShape derived class, such as b3BvhTriangleMeshShape, b3StaticPlaneShape and b3HeightfieldTerrainShape. +class b3TriangleCallback +{ +public: + + virtual ~b3TriangleCallback(); + virtual void processTriangle(b3Vector3* triangle, int partId, int triangleIndex) = 0; +}; + +class b3InternalTriangleIndexCallback +{ +public: + + virtual ~b3InternalTriangleIndexCallback(); + virtual void internalProcessTriangleIndex(b3Vector3* triangle,int partId,int triangleIndex) = 0; +}; + + + +#endif //B3_TRIANGLE_CALLBACK_H diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.cpp b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.cpp new file mode 100644 index 000000000000..a0f59babbee1 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.cpp @@ -0,0 +1,95 @@ +/* +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 "b3TriangleIndexVertexArray.h" + +b3TriangleIndexVertexArray::b3TriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,b3Scalar* vertexBase,int vertexStride) +: m_hasAabb(0) +{ + b3IndexedMesh mesh; + + mesh.m_numTriangles = numTriangles; + mesh.m_triangleIndexBase = (const unsigned char *)triangleIndexBase; + mesh.m_triangleIndexStride = triangleIndexStride; + mesh.m_numVertices = numVertices; + mesh.m_vertexBase = (const unsigned char *)vertexBase; + mesh.m_vertexStride = vertexStride; + + addIndexedMesh(mesh); + +} + +b3TriangleIndexVertexArray::~b3TriangleIndexVertexArray() +{ + +} + +void b3TriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) +{ + b3Assert(subpart< getNumSubParts() ); + + b3IndexedMesh& mesh = m_indexedMeshes[subpart]; + + numverts = mesh.m_numVertices; + (*vertexbase) = (unsigned char *) mesh.m_vertexBase; + + type = mesh.m_vertexType; + + vertexStride = mesh.m_vertexStride; + + numfaces = mesh.m_numTriangles; + + (*indexbase) = (unsigned char *)mesh.m_triangleIndexBase; + indexstride = mesh.m_triangleIndexStride; + indicestype = mesh.m_indexType; +} + +void b3TriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const +{ + const b3IndexedMesh& mesh = m_indexedMeshes[subpart]; + + numverts = mesh.m_numVertices; + (*vertexbase) = (const unsigned char *)mesh.m_vertexBase; + + type = mesh.m_vertexType; + + vertexStride = mesh.m_vertexStride; + + numfaces = mesh.m_numTriangles; + (*indexbase) = (const unsigned char *)mesh.m_triangleIndexBase; + indexstride = mesh.m_triangleIndexStride; + indicestype = mesh.m_indexType; +} + +bool b3TriangleIndexVertexArray::hasPremadeAabb() const +{ + return (m_hasAabb == 1); +} + + +void b3TriangleIndexVertexArray::setPremadeAabb(const b3Vector3& aabbMin, const b3Vector3& aabbMax ) const +{ + m_aabbMin = aabbMin; + m_aabbMax = aabbMax; + m_hasAabb = 1; // this is intentionally an int see notes in header +} + +void b3TriangleIndexVertexArray::getPremadeAabb(b3Vector3* aabbMin, b3Vector3* aabbMax ) const +{ + *aabbMin = m_aabbMin; + *aabbMax = m_aabbMax; +} + + diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h new file mode 100644 index 000000000000..d26b2893bc57 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h @@ -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. +*/ + +#ifndef B3_TRIANGLE_INDEX_VERTEX_ARRAY_H +#define B3_TRIANGLE_INDEX_VERTEX_ARRAY_H + +#include "b3StridingMeshInterface.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Scalar.h" + + +///The b3IndexedMesh indexes a single vertex and index array. Multiple b3IndexedMesh objects can be passed into a b3TriangleIndexVertexArray using addIndexedMesh. +///Instead of the number of indices, we pass the number of triangles. +B3_ATTRIBUTE_ALIGNED16( struct) b3IndexedMesh +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); + + int m_numTriangles; + const unsigned char * m_triangleIndexBase; + // Size in byte of the indices for one triangle (3*sizeof(index_type) if the indices are tightly packed) + int m_triangleIndexStride; + int m_numVertices; + const unsigned char * m_vertexBase; + // Size of a vertex, in bytes + int m_vertexStride; + + // The index type is set when adding an indexed mesh to the + // b3TriangleIndexVertexArray, do not set it manually + PHY_ScalarType m_indexType; + + // The vertex type has a default type similar to Bullet's precision mode (float or double) + // but can be set manually if you for example run Bullet with double precision but have + // mesh data in single precision.. + PHY_ScalarType m_vertexType; + + + b3IndexedMesh() + :m_indexType(PHY_INTEGER), +#ifdef B3_USE_DOUBLE_PRECISION + m_vertexType(PHY_DOUBLE) +#else // B3_USE_DOUBLE_PRECISION + m_vertexType(PHY_FLOAT) +#endif // B3_USE_DOUBLE_PRECISION + { + } +} +; + + +typedef b3AlignedObjectArray IndexedMeshArray; + +///The b3TriangleIndexVertexArray allows to access multiple triangle meshes, by indexing into existing triangle/index arrays. +///Additional meshes can be added using addIndexedMesh +///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays. +///So keep those arrays around during the lifetime of this b3TriangleIndexVertexArray. +B3_ATTRIBUTE_ALIGNED16( class) b3TriangleIndexVertexArray : public b3StridingMeshInterface +{ +protected: + IndexedMeshArray m_indexedMeshes; + int m_pad[2]; + mutable int m_hasAabb; // using int instead of bool to maintain alignment + mutable b3Vector3 m_aabbMin; + mutable b3Vector3 m_aabbMax; + +public: + + B3_DECLARE_ALIGNED_ALLOCATOR(); + + b3TriangleIndexVertexArray() : m_hasAabb(0) + { + } + + virtual ~b3TriangleIndexVertexArray(); + + //just to be backwards compatible + b3TriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,b3Scalar* vertexBase,int vertexStride); + + void addIndexedMesh(const b3IndexedMesh& mesh, PHY_ScalarType indexType = PHY_INTEGER) + { + m_indexedMeshes.push_back(mesh); + m_indexedMeshes[m_indexedMeshes.size()-1].m_indexType = indexType; + } + + + virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0); + + virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const; + + /// unLockVertexBase finishes the access to a subpart of the triangle mesh + /// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished + virtual void unLockVertexBase(int subpart) {(void)subpart;} + + virtual void unLockReadOnlyVertexBase(int subpart) const {(void)subpart;} + + /// getNumSubParts returns the number of seperate subparts + /// each subpart has a continuous array of vertices and indices + virtual int getNumSubParts() const { + return (int)m_indexedMeshes.size(); + } + + IndexedMeshArray& getIndexedMeshArray() + { + return m_indexedMeshes; + } + + const IndexedMeshArray& getIndexedMeshArray() const + { + return m_indexedMeshes; + } + + virtual void preallocateVertices(int numverts){(void) numverts;} + virtual void preallocateIndices(int numindices){(void) numindices;} + + virtual bool hasPremadeAabb() const; + virtual void setPremadeAabb(const b3Vector3& aabbMin, const b3Vector3& aabbMax ) const; + virtual void getPremadeAabb(b3Vector3* aabbMin, b3Vector3* aabbMax ) const; + +} +; + +#endif //B3_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h new file mode 100644 index 000000000000..f6f65f7719d6 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h @@ -0,0 +1,11 @@ +#ifndef B3_VECTOR_FLOAT4_H +#define B3_VECTOR_FLOAT4_H + +#include "Bullet3Common/b3Transform.h" + +//#define cross3(a,b) (a.cross(b)) +#define float4 b3Vector3 +//#define make_float4(x,y,z,w) b3Vector4(x,y,z,w) + + +#endif //B3_VECTOR_FLOAT4_H diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp new file mode 100644 index 000000000000..cf3d5ef49d4a --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp @@ -0,0 +1,609 @@ + +/* +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. + + Elsevier CDROM license agreements grants nonexclusive license to use the software + for any purpose, commercial or non-commercial as long as the following credit is included + identifying the original source of the software: + + Parts of the source are "from the book Real-Time Collision Detection by + Christer Ericson, published by Morgan Kaufmann Publishers, + (c) 2005 Elsevier Inc." + +*/ + + +#include "b3VoronoiSimplexSolver.h" + +#define VERTA 0 +#define VERTB 1 +#define VERTC 2 +#define VERTD 3 + +#define B3_CATCH_DEGENERATE_TETRAHEDRON 1 +void b3VoronoiSimplexSolver::removeVertex(int index) +{ + + b3Assert(m_numVertices>0); + m_numVertices--; + m_simplexVectorW[index] = m_simplexVectorW[m_numVertices]; + m_simplexPointsP[index] = m_simplexPointsP[m_numVertices]; + m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices]; +} + +void b3VoronoiSimplexSolver::reduceVertices (const b3UsageBitfield& usedVerts) +{ + if ((numVertices() >= 4) && (!usedVerts.usedVertexD)) + removeVertex(3); + + if ((numVertices() >= 3) && (!usedVerts.usedVertexC)) + removeVertex(2); + + if ((numVertices() >= 2) && (!usedVerts.usedVertexB)) + removeVertex(1); + + if ((numVertices() >= 1) && (!usedVerts.usedVertexA)) + removeVertex(0); + +} + + + + + +//clear the simplex, remove all the vertices +void b3VoronoiSimplexSolver::reset() +{ + m_cachedValidClosest = false; + m_numVertices = 0; + m_needsUpdate = true; + m_lastW = b3MakeVector3(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT)); + m_cachedBC.reset(); +} + + + + //add a vertex +void b3VoronoiSimplexSolver::addVertex(const b3Vector3& w, const b3Vector3& p, const b3Vector3& q) +{ + m_lastW = w; + m_needsUpdate = true; + + m_simplexVectorW[m_numVertices] = w; + m_simplexPointsP[m_numVertices] = p; + m_simplexPointsQ[m_numVertices] = q; + + m_numVertices++; +} + +bool b3VoronoiSimplexSolver::updateClosestVectorAndPoints() +{ + + if (m_needsUpdate) + { + m_cachedBC.reset(); + + m_needsUpdate = false; + + switch (numVertices()) + { + case 0: + m_cachedValidClosest = false; + break; + case 1: + { + m_cachedP1 = m_simplexPointsP[0]; + m_cachedP2 = m_simplexPointsQ[0]; + m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] + m_cachedBC.reset(); + m_cachedBC.setBarycentricCoordinates(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + m_cachedValidClosest = m_cachedBC.isValid(); + break; + }; + case 2: + { + //closest point origin from line segment + const b3Vector3& from = m_simplexVectorW[0]; + const b3Vector3& to = m_simplexVectorW[1]; + b3Vector3 nearest; + + b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + b3Vector3 diff = p - from; + b3Vector3 v = to - from; + b3Scalar t = v.dot(diff); + + if (t > 0) { + b3Scalar dotVV = v.dot(v); + if (t < dotVV) { + t /= dotVV; + diff -= t*v; + m_cachedBC.m_usedVertices.usedVertexA = true; + m_cachedBC.m_usedVertices.usedVertexB = true; + } else { + t = 1; + diff -= v; + //reduce to 1 point + m_cachedBC.m_usedVertices.usedVertexB = true; + } + } else + { + t = 0; + //reduce to 1 point + m_cachedBC.m_usedVertices.usedVertexA = true; + } + m_cachedBC.setBarycentricCoordinates(1-t,t); + nearest = from + t*v; + + m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); + m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); + m_cachedV = m_cachedP1 - m_cachedP2; + + reduceVertices(m_cachedBC.m_usedVertices); + + m_cachedValidClosest = m_cachedBC.isValid(); + break; + } + case 3: + { + //closest point origin from triangle + b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + + const b3Vector3& a = m_simplexVectorW[0]; + const b3Vector3& b = m_simplexVectorW[1]; + const b3Vector3& c = m_simplexVectorW[2]; + + closestPtPointTriangle(p,a,b,c,m_cachedBC); + m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; + + m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; + + m_cachedV = m_cachedP1-m_cachedP2; + + reduceVertices (m_cachedBC.m_usedVertices); + m_cachedValidClosest = m_cachedBC.isValid(); + + break; + } + case 4: + { + + + b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + + const b3Vector3& a = m_simplexVectorW[0]; + const b3Vector3& b = m_simplexVectorW[1]; + const b3Vector3& c = m_simplexVectorW[2]; + const b3Vector3& d = m_simplexVectorW[3]; + + bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); + + if (hasSeperation) + { + + m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; + + m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; + + m_cachedV = m_cachedP1-m_cachedP2; + reduceVertices (m_cachedBC.m_usedVertices); + } else + { +// printf("sub distance got penetration\n"); + + if (m_cachedBC.m_degenerate) + { + m_cachedValidClosest = false; + } else + { + m_cachedValidClosest = true; + //degenerate case == false, penetration = true + zero + m_cachedV.setValue(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + } + break; + } + + m_cachedValidClosest = m_cachedBC.isValid(); + + //closest point origin from tetrahedron + break; + } + default: + { + m_cachedValidClosest = false; + } + }; + } + + return m_cachedValidClosest; + +} + +//return/calculate the closest vertex +bool b3VoronoiSimplexSolver::closest(b3Vector3& v) +{ + bool succes = updateClosestVectorAndPoints(); + v = m_cachedV; + return succes; +} + + + +b3Scalar b3VoronoiSimplexSolver::maxVertex() +{ + int i, numverts = numVertices(); + b3Scalar maxV = b3Scalar(0.); + for (i=0;i= b3Scalar(0.0) && d4 <= d3) + { + result.m_closestPointOnSimplex = b; + result.m_usedVertices.usedVertexB = true; + result.setBarycentricCoordinates(0,1,0); + + return true; // b; // barycentric coordinates (0,1,0) + } + // Check if P in edge region of AB, if so return projection of P onto AB + b3Scalar vc = d1*d4 - d3*d2; + if (vc <= b3Scalar(0.0) && d1 >= b3Scalar(0.0) && d3 <= b3Scalar(0.0)) { + b3Scalar v = d1 / (d1 - d3); + result.m_closestPointOnSimplex = a + v * ab; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexB = true; + result.setBarycentricCoordinates(1-v,v,0); + return true; + //return a + v * ab; // barycentric coordinates (1-v,v,0) + } + + // Check if P in vertex region outside C + b3Vector3 cp = p - c; + b3Scalar d5 = ab.dot(cp); + b3Scalar d6 = ac.dot(cp); + if (d6 >= b3Scalar(0.0) && d5 <= d6) + { + result.m_closestPointOnSimplex = c; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(0,0,1); + return true;//c; // barycentric coordinates (0,0,1) + } + + // Check if P in edge region of AC, if so return projection of P onto AC + b3Scalar vb = d5*d2 - d1*d6; + if (vb <= b3Scalar(0.0) && d2 >= b3Scalar(0.0) && d6 <= b3Scalar(0.0)) { + b3Scalar w = d2 / (d2 - d6); + result.m_closestPointOnSimplex = a + w * ac; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(1-w,0,w); + return true; + //return a + w * ac; // barycentric coordinates (1-w,0,w) + } + + // Check if P in edge region of BC, if so return projection of P onto BC + b3Scalar va = d3*d6 - d5*d4; + if (va <= b3Scalar(0.0) && (d4 - d3) >= b3Scalar(0.0) && (d5 - d6) >= b3Scalar(0.0)) { + b3Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + + result.m_closestPointOnSimplex = b + w * (c - b); + result.m_usedVertices.usedVertexB = true; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(0,1-w,w); + return true; + // return b + w * (c - b); // barycentric coordinates (0,1-w,w) + } + + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + b3Scalar denom = b3Scalar(1.0) / (va + vb + vc); + b3Scalar v = vb * denom; + b3Scalar w = vc * denom; + + result.m_closestPointOnSimplex = a + ab * v + ac * w; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexB = true; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(1-v-w,v,w); + + return true; +// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = b3Scalar(1.0) - v - w + +} + + + + + +/// Test if point p and d lie on opposite sides of plane through abc +int b3VoronoiSimplexSolver::pointOutsideOfPlane(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d) +{ + b3Vector3 normal = (b-a).cross(c-a); + + b3Scalar signp = (p - a).dot(normal); // [AP AB AC] + b3Scalar signd = (d - a).dot( normal); // [AD AB AC] + +#ifdef B3_CATCH_DEGENERATE_TETRAHEDRON +#ifdef BT_USE_DOUBLE_PRECISION +if (signd * signd < (b3Scalar(1e-8) * b3Scalar(1e-8))) + { + return -1; + } +#else + if (signd * signd < (b3Scalar(1e-4) * b3Scalar(1e-4))) + { +// printf("affine dependent/degenerate\n");// + return -1; + } +#endif + +#endif + // Points on opposite sides if expression signs are opposite + return signp * signd < b3Scalar(0.); +} + + +bool b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d, b3SubSimplexClosestResult& finalResult) +{ + b3SubSimplexClosestResult tempResult; + + // Start out assuming point inside all halfspaces, so closest to itself + finalResult.m_closestPointOnSimplex = p; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = true; + finalResult.m_usedVertices.usedVertexB = true; + finalResult.m_usedVertices.usedVertexC = true; + finalResult.m_usedVertices.usedVertexD = true; + + int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d); + int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b); + int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c); + int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a); + + if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) + { + finalResult.m_degenerate = true; + return false; + } + + if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) + { + return false; + } + + + b3Scalar bestSqDist = FLT_MAX; + // If point outside face abc then compute closest point on abc + if (pointOutsideABC) + { + closestPtPointTriangle(p, a, b, c,tempResult); + b3Vector3 q = tempResult.m_closestPointOnSimplex; + + b3Scalar sqDist = (q - p).dot( q - p); + // Update best closest point if (squared) distance is less than current best + if (sqDist < bestSqDist) { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + //convert result bitmask! + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; + finalResult.setBarycentricCoordinates( + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC], + 0 + ); + + } + } + + + // Repeat test for face acd + if (pointOutsideACD) + { + closestPtPointTriangle(p, a, c, d,tempResult); + b3Vector3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + + b3Scalar sqDist = (q - p).dot( q - p); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; + finalResult.setBarycentricCoordinates( + tempResult.m_barycentricCoords[VERTA], + 0, + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC] + ); + + } + } + // Repeat test for face adb + + + if (pointOutsideADB) + { + closestPtPointTriangle(p, a, d, b,tempResult); + b3Vector3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + + b3Scalar sqDist = (q - p).dot( q - p); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; + + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; + finalResult.setBarycentricCoordinates( + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTC], + 0, + tempResult.m_barycentricCoords[VERTB] + ); + + } + } + // Repeat test for face bdc + + + if (pointOutsideBDC) + { + closestPtPointTriangle(p, b, d, c,tempResult); + b3Vector3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + b3Scalar sqDist = (q - p).dot( q - p); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + finalResult.m_usedVertices.reset(); + // + finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; + + finalResult.setBarycentricCoordinates( + 0, + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTC], + tempResult.m_barycentricCoords[VERTB] + ); + + } + } + + //help! we ended up full ! + + if (finalResult.m_usedVertices.usedVertexA && + finalResult.m_usedVertices.usedVertexB && + finalResult.m_usedVertices.usedVertexC && + finalResult.m_usedVertices.usedVertexD) + { + return true; + } + + return true; +} + diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.h new file mode 100644 index 000000000000..a6e27667d89e --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.h @@ -0,0 +1,177 @@ +/* +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 B3_VORONOI_SIMPLEX_SOLVER_H +#define B3_VORONOI_SIMPLEX_SOLVER_H + +#include "Bullet3Common/b3Vector3.h" + + +#define VORONOI_SIMPLEX_MAX_VERTS 5 + +///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure +//#define BT_USE_EQUAL_VERTEX_THRESHOLD +#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f + + +struct b3UsageBitfield{ + b3UsageBitfield() + { + reset(); + } + + void reset() + { + usedVertexA = false; + usedVertexB = false; + usedVertexC = false; + usedVertexD = false; + } + unsigned short usedVertexA : 1; + unsigned short usedVertexB : 1; + unsigned short usedVertexC : 1; + unsigned short usedVertexD : 1; + unsigned short unused1 : 1; + unsigned short unused2 : 1; + unsigned short unused3 : 1; + unsigned short unused4 : 1; +}; + + +struct b3SubSimplexClosestResult +{ + b3Vector3 m_closestPointOnSimplex; + //MASK for m_usedVertices + //stores the simplex vertex-usage, using the MASK, + // if m_usedVertices & MASK then the related vertex is used + b3UsageBitfield m_usedVertices; + b3Scalar m_barycentricCoords[4]; + bool m_degenerate; + + void reset() + { + m_degenerate = false; + setBarycentricCoordinates(); + m_usedVertices.reset(); + } + bool isValid() + { + bool valid = (m_barycentricCoords[0] >= b3Scalar(0.)) && + (m_barycentricCoords[1] >= b3Scalar(0.)) && + (m_barycentricCoords[2] >= b3Scalar(0.)) && + (m_barycentricCoords[3] >= b3Scalar(0.)); + + + return valid; + } + void setBarycentricCoordinates(b3Scalar a=b3Scalar(0.),b3Scalar b=b3Scalar(0.),b3Scalar c=b3Scalar(0.),b3Scalar d=b3Scalar(0.)) + { + m_barycentricCoords[0] = a; + m_barycentricCoords[1] = b; + m_barycentricCoords[2] = c; + m_barycentricCoords[3] = d; + } + +}; + +/// b3VoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin. +/// Can be used with GJK, as an alternative to Johnson distance algorithm. + +B3_ATTRIBUTE_ALIGNED16(class) b3VoronoiSimplexSolver +{ +public: + + B3_DECLARE_ALIGNED_ALLOCATOR(); + + int m_numVertices; + + b3Vector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; + b3Vector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; + b3Vector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; + + + + b3Vector3 m_cachedP1; + b3Vector3 m_cachedP2; + b3Vector3 m_cachedV; + b3Vector3 m_lastW; + + b3Scalar m_equalVertexThreshold; + bool m_cachedValidClosest; + + + b3SubSimplexClosestResult m_cachedBC; + + bool m_needsUpdate; + + void removeVertex(int index); + void reduceVertices (const b3UsageBitfield& usedVerts); + bool updateClosestVectorAndPoints(); + + bool closestPtPointTetrahedron(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d, b3SubSimplexClosestResult& finalResult); + int pointOutsideOfPlane(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d); + bool closestPtPointTriangle(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c,b3SubSimplexClosestResult& result); + +public: + + b3VoronoiSimplexSolver() + : m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD) + { + } + void reset(); + + void addVertex(const b3Vector3& w, const b3Vector3& p, const b3Vector3& q); + + void setEqualVertexThreshold(b3Scalar threshold) + { + m_equalVertexThreshold = threshold; + } + + b3Scalar getEqualVertexThreshold() const + { + return m_equalVertexThreshold; + } + + bool closest(b3Vector3& v); + + b3Scalar maxVertex(); + + bool fullSimplex() const + { + return (m_numVertices == 4); + } + + int getSimplex(b3Vector3 *pBuf, b3Vector3 *qBuf, b3Vector3 *yBuf) const; + + bool inSimplex(const b3Vector3& w); + + void backup_closest(b3Vector3& v) ; + + bool emptySimplex() const ; + + void compute_points(b3Vector3& p1, b3Vector3& p2) ; + + int numVertices() const + { + return m_numVertices; + } + + +}; + +#endif //B3_VORONOI_SIMPLEX_SOLVER_H + diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl new file mode 100644 index 000000000000..faa413441c55 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl @@ -0,0 +1,283 @@ +//keep this enum in sync with the CPU version (in btCollidable.h) +//written by Erwin Coumans + +#define SHAPE_CONVEX_HULL 3 +#define SHAPE_CONCAVE_TRIMESH 5 +#define TRIANGLE_NUM_CONVEX_FACES 5 +#define SHAPE_COMPOUND_OF_CONVEX_HULLS 6 +#define SHAPE_SPHERE 7 + +typedef unsigned int u32; + +#define MAX_NUM_PARTS_IN_BITS 10 + +///btQuantizedBvhNode is a compressed aabb node, 16 bytes. +///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). +typedef struct +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes + int m_escapeIndexOrTriangleIndex; +} btQuantizedBvhNode; + +typedef struct +{ + float4 m_aabbMin; + float4 m_aabbMax; + float4 m_quantization; + int m_numNodes; + int m_numSubTrees; + int m_nodeOffset; + int m_subTreeOffset; + +} b3BvhInfo; + +int getTriangleIndex(const btQuantizedBvhNode* rootNode) +{ + unsigned int x=0; + unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS); + // Get only the lower bits where the triangle index is stored + return (rootNode->m_escapeIndexOrTriangleIndex&~(y)); +} + +int isLeaf(const btQuantizedBvhNode* rootNode) +{ + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0; +} + +int getEscapeIndex(const btQuantizedBvhNode* rootNode) +{ + return -rootNode->m_escapeIndexOrTriangleIndex; +} + +typedef struct +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes, points to the root of the subtree + int m_rootNodeIndex; + //4 bytes + int m_subtreeSize; + int m_padding[3]; +} btBvhSubtreeInfo; + +///keep this in sync with btCollidable.h +typedef struct +{ + int m_numChildShapes; + int blaat2; + int m_shapeType; + int m_shapeIndex; + +} btCollidableGpu; + +typedef struct +{ + float4 m_childPosition; + float4 m_childOrientation; + int m_shapeIndex; + int m_unused0; + int m_unused1; + int m_unused2; +} btGpuChildShape; + + +typedef struct +{ + float4 m_pos; + float4 m_quat; + float4 m_linVel; + float4 m_angVel; + + u32 m_collidableIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} BodyData; + +typedef struct +{ + union + { + float4 m_min; + float m_minElems[4]; + int m_minIndices[4]; + }; + union + { + float4 m_max; + float m_maxElems[4]; + int m_maxIndices[4]; + }; +} btAabbCL; + + +int testQuantizedAabbAgainstQuantizedAabb( + const unsigned short int* aabbMin1, + const unsigned short int* aabbMax1, + const unsigned short int* aabbMin2, + const unsigned short int* aabbMax2) +{ + //int overlap = 1; + if (aabbMin1[0] > aabbMax2[0]) + return 0; + if (aabbMax1[0] < aabbMin2[0]) + return 0; + if (aabbMin1[1] > aabbMax2[1]) + return 0; + if (aabbMax1[1] < aabbMin2[1]) + return 0; + if (aabbMin1[2] > aabbMax2[2]) + return 0; + if (aabbMax1[2] < aabbMin2[2]) + return 0; + return 1; + //overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap; + //overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap; + //overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap; + //return overlap; +} + + +void quantizeWithClamp(unsigned short* out, float4 point2,int isMax, float4 bvhAabbMin, float4 bvhAabbMax, float4 bvhQuantization) +{ + float4 clampedPoint = max(point2,bvhAabbMin); + clampedPoint = min (clampedPoint, bvhAabbMax); + + float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization; + if (isMax) + { + out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1)); + out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1)); + out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1)); + } else + { + out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe)); + out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe)); + out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe)); + } + +} + + +// work-in-progress +__kernel void bvhTraversalKernel( __global const int4* pairs, + __global const BodyData* rigidBodies, + __global const btCollidableGpu* collidables, + __global btAabbCL* aabbs, + __global int4* concavePairsOut, + __global volatile int* numConcavePairsOut, + __global const btBvhSubtreeInfo* subtreeHeadersRoot, + __global const btQuantizedBvhNode* quantizedNodesRoot, + __global const b3BvhInfo* bvhInfos, + int numPairs, + int maxNumConcavePairsCapacity) +{ + int id = get_global_id(0); + if (id>=numPairs) + return; + + int bodyIndexA = pairs[id].x; + int bodyIndexB = pairs[id].y; + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + //once the broadphase avoids static-static pairs, we can remove this test + if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0)) + { + return; + } + + if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH) + return; + + int shapeTypeB = collidables[collidableIndexB].m_shapeType; + + if (shapeTypeB!=SHAPE_CONVEX_HULL && + shapeTypeB!=SHAPE_SPHERE && + shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS + ) + return; + + b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes]; + + float4 bvhAabbMin = bvhInfo.m_aabbMin; + float4 bvhAabbMax = bvhInfo.m_aabbMax; + float4 bvhQuantization = bvhInfo.m_quantization; + int numSubtreeHeaders = bvhInfo.m_numSubTrees; + __global const btBvhSubtreeInfo* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset]; + __global const btQuantizedBvhNode* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset]; + + + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + quantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_min,false,bvhAabbMin, bvhAabbMax,bvhQuantization); + quantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_max,true ,bvhAabbMin, bvhAabbMax,bvhQuantization); + + for (int i=0;im_escapeIndexOrTriangleIndex&~(y));\n" +"}\n" +"int isLeaf(const btQuantizedBvhNode* rootNode)\n" +"{\n" +" //skipindex is negative (internal node), triangleindex >=0 (leafnode)\n" +" return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;\n" +"}\n" +" \n" +"int getEscapeIndex(const btQuantizedBvhNode* rootNode)\n" +"{\n" +" return -rootNode->m_escapeIndexOrTriangleIndex;\n" +"}\n" +"typedef struct\n" +"{\n" +" //12 bytes\n" +" unsigned short int m_quantizedAabbMin[3];\n" +" unsigned short int m_quantizedAabbMax[3];\n" +" //4 bytes, points to the root of the subtree\n" +" int m_rootNodeIndex;\n" +" //4 bytes\n" +" int m_subtreeSize;\n" +" int m_padding[3];\n" +"} btBvhSubtreeInfo;\n" +"///keep this in sync with btCollidable.h\n" +"typedef struct\n" +"{\n" +" int m_numChildShapes;\n" +" int blaat2;\n" +" int m_shapeType;\n" +" int m_shapeIndex;\n" +" \n" +"} btCollidableGpu;\n" +"typedef struct\n" +"{\n" +" float4 m_childPosition;\n" +" float4 m_childOrientation;\n" +" int m_shapeIndex;\n" +" int m_unused0;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"} btGpuChildShape;\n" +"typedef struct\n" +"{\n" +" float4 m_pos;\n" +" float4 m_quat;\n" +" float4 m_linVel;\n" +" float4 m_angVel;\n" +" u32 m_collidableIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"} BodyData;\n" +"typedef struct \n" +"{\n" +" union\n" +" {\n" +" float4 m_min;\n" +" float m_minElems[4];\n" +" int m_minIndices[4];\n" +" };\n" +" union\n" +" {\n" +" float4 m_max;\n" +" float m_maxElems[4];\n" +" int m_maxIndices[4];\n" +" };\n" +"} btAabbCL;\n" +"int testQuantizedAabbAgainstQuantizedAabb(\n" +" const unsigned short int* aabbMin1,\n" +" const unsigned short int* aabbMax1,\n" +" const unsigned short int* aabbMin2,\n" +" const unsigned short int* aabbMax2)\n" +"{\n" +" //int overlap = 1;\n" +" if (aabbMin1[0] > aabbMax2[0])\n" +" return 0;\n" +" if (aabbMax1[0] < aabbMin2[0])\n" +" return 0;\n" +" if (aabbMin1[1] > aabbMax2[1])\n" +" return 0;\n" +" if (aabbMax1[1] < aabbMin2[1])\n" +" return 0;\n" +" if (aabbMin1[2] > aabbMax2[2])\n" +" return 0;\n" +" if (aabbMax1[2] < aabbMin2[2])\n" +" return 0;\n" +" return 1;\n" +" //overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap;\n" +" //overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap;\n" +" //overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap;\n" +" //return overlap;\n" +"}\n" +"void quantizeWithClamp(unsigned short* out, float4 point2,int isMax, float4 bvhAabbMin, float4 bvhAabbMax, float4 bvhQuantization)\n" +"{\n" +" float4 clampedPoint = max(point2,bvhAabbMin);\n" +" clampedPoint = min (clampedPoint, bvhAabbMax);\n" +" float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;\n" +" if (isMax)\n" +" {\n" +" out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1));\n" +" out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1));\n" +" out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1));\n" +" } else\n" +" {\n" +" out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe));\n" +" out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe));\n" +" out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe));\n" +" }\n" +"}\n" +"// work-in-progress\n" +"__kernel void bvhTraversalKernel( __global const int4* pairs, \n" +" __global const BodyData* rigidBodies, \n" +" __global const btCollidableGpu* collidables,\n" +" __global btAabbCL* aabbs,\n" +" __global int4* concavePairsOut,\n" +" __global volatile int* numConcavePairsOut,\n" +" __global const btBvhSubtreeInfo* subtreeHeadersRoot,\n" +" __global const btQuantizedBvhNode* quantizedNodesRoot,\n" +" __global const b3BvhInfo* bvhInfos,\n" +" int numPairs,\n" +" int maxNumConcavePairsCapacity)\n" +"{\n" +" int id = get_global_id(0);\n" +" if (id>=numPairs)\n" +" return;\n" +" \n" +" int bodyIndexA = pairs[id].x;\n" +" int bodyIndexB = pairs[id].y;\n" +" int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" +" int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;\n" +" \n" +" //once the broadphase avoids static-static pairs, we can remove this test\n" +" if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))\n" +" {\n" +" return;\n" +" }\n" +" \n" +" if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH)\n" +" return;\n" +" int shapeTypeB = collidables[collidableIndexB].m_shapeType;\n" +" \n" +" if (shapeTypeB!=SHAPE_CONVEX_HULL &&\n" +" shapeTypeB!=SHAPE_SPHERE &&\n" +" shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS\n" +" )\n" +" return;\n" +" b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];\n" +" float4 bvhAabbMin = bvhInfo.m_aabbMin;\n" +" float4 bvhAabbMax = bvhInfo.m_aabbMax;\n" +" float4 bvhQuantization = bvhInfo.m_quantization;\n" +" int numSubtreeHeaders = bvhInfo.m_numSubTrees;\n" +" __global const btBvhSubtreeInfo* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];\n" +" __global const btQuantizedBvhNode* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];\n" +" \n" +" unsigned short int quantizedQueryAabbMin[3];\n" +" unsigned short int quantizedQueryAabbMax[3];\n" +" quantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_min,false,bvhAabbMin, bvhAabbMax,bvhQuantization);\n" +" quantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_max,true ,bvhAabbMin, bvhAabbMax,bvhQuantization);\n" +" \n" +" for (int i=0;im_worldNormalOnB = -dirOut;//normal; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + int bodyA = pairs[pairIndex].x; + int bodyB = pairs[pairIndex].y; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0 ? -bodyA:bodyA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0 ? -bodyB:bodyB; + c->m_childIndexA = -1; + c->m_childIndexB = -1; + //for (int i=0;im_worldPosB[0] = posOut;//localPoints[contactIdx[i]]; + GET_NPOINTS(*c) = 1;//nContacts; + } + } + + } +} + +typedef float4 Quaternion; +#define make_float4 (float4) + +__inline +float dot3F4(float4 a, float4 b) +{ + float4 a1 = make_float4(a.xyz,0.f); + float4 b1 = make_float4(b.xyz,0.f); + return dot(a1, b1); +} + + + + +__inline +float4 cross3(float4 a, float4 b) +{ + return cross(a,b); +} +__inline +Quaternion qtMul(Quaternion a, Quaternion b) +{ + Quaternion ans; + ans = cross3( a, b ); + ans += a.w*b+b.w*a; +// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - dot3F4(a, b); + return ans; +} + +__inline +Quaternion qtInvert(Quaternion q) +{ + return (Quaternion)(-q.xyz, q.w); +} + +__inline +float4 qtRotate(Quaternion q, float4 vec) +{ + Quaternion qInv = qtInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = qtMul(qtMul(q,vcpy),qInv); + return out; +} + +__inline +float4 transform(const float4* p, const float4* translation, const Quaternion* orientation) +{ + return qtRotate( *orientation, *p ) + (*translation); +} + + +__inline +float4 qtInvRotate(const Quaternion q, float4 vec) +{ + return qtRotate( qtInvert( q ), vec ); +} + + +inline void project(__global const b3ConvexPolyhedronData_t* hull, const float4 pos, const float4 orn, +const float4* dir, __global const float4* vertices, float* min, float* max) +{ + min[0] = FLT_MAX; + max[0] = -FLT_MAX; + int numVerts = hull->m_numVertices; + + const float4 localDir = qtInvRotate(orn,*dir); + float offset = dot(pos,*dir); + for(int i=0;im_vertexOffset+i],localDir); + if(dp < min[0]) + min[0] = dp; + if(dp > max[0]) + max[0] = dp; + } + if(min[0]>max[0]) + { + float tmp = min[0]; + min[0] = max[0]; + max[0] = tmp; + } + min[0] += offset; + max[0] += offset; +} + + +bool findSeparatingAxisUnitSphere( __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB, + const float4 posA1, + const float4 ornA, + const float4 posB1, + const float4 ornB, + const float4 DeltaC2, + __global const float4* vertices, + __global const float4* unitSphereDirections, + int numUnitSphereDirections, + float4* sep, + float* dmin) +{ + + float4 posA = posA1; + posA.w = 0.f; + float4 posB = posB1; + posB.w = 0.f; + + int curPlaneTests=0; + + int curEdgeEdge = 0; + // Test unit sphere directions + for (int i=0;i0) + crossje *= -1.f; + { + float dist; + bool result = true; + float Min0,Max0; + float Min1,Max1; + project(hullA,posA,ornA,&crossje,vertices, &Min0, &Max0); + project(hullB,posB,ornB,&crossje,vertices, &Min1, &Max1); + + if(Max00.0f) + { + *sep = -(*sep); + } + return true; +} + + + +__kernel void findSeparatingAxisUnitSphereKernel( __global const int4* pairs, + __global const b3RigidBodyData_t* rigidBodies, + __global const b3Collidable_t* collidables, + __global const b3ConvexPolyhedronData_t* convexShapes, + __global const float4* vertices, + __global const float4* unitSphereDirections, + __global float4* separatingNormals, + __global int* hasSeparatingAxis, + __global float* dmins, + int numUnitSphereDirections, + int numPairs + ) +{ + + int i = get_global_id(0); + + if (inumUnitSphereDirections) + { + bool sepEE = findSeparatingAxisUnitSphere( &convexShapes[shapeIndexA], &convexShapes[shapeIndexB],posA,ornA, + posB,ornB, + DeltaC2, + vertices,unitSphereDirections,numUnitSphereDirections,&sepNormal,&dmin); + if (!sepEE) + { + hasSeparatingAxis[i] = 0; + } else + { + hasSeparatingAxis[i] = 1; + separatingNormals[i] = sepNormal; + } + } + } //if (hasSeparatingAxis[i]) + }//(i\n" +" *\n" +" * This file was ported from mpr.c file, part of libccd.\n" +" * The Minkoski Portal Refinement implementation was ported \n" +" * to OpenCL by Erwin Coumans for the Bullet 3 Physics library.\n" +" * at http://github.com/erwincoumans/bullet3\n" +" *\n" +" * Distributed under the OSI-approved BSD License (the \"License\");\n" +" * see .\n" +" * This software is distributed WITHOUT ANY WARRANTY; without even the\n" +" * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\n" +" * See the License for more information.\n" +" */\n" +"#ifndef B3_MPR_PENETRATION_H\n" +"#define B3_MPR_PENETRATION_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#define B3_PLATFORM_DEFINITIONS_H\n" +"struct MyTest\n" +"{\n" +" int bla;\n" +"};\n" +"#ifdef __cplusplus\n" +"#else\n" +"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" +"#define B3_LARGE_FLOAT 1e18f\n" +"#define B3_INFINITY 1e18f\n" +"#define b3Assert(a)\n" +"#define b3ConstArray(a) __global const a*\n" +"#define b3AtomicInc atomic_inc\n" +"#define b3AtomicAdd atomic_add\n" +"#define b3Fabs fabs\n" +"#define b3Sqrt native_sqrt\n" +"#define b3Sin native_sin\n" +"#define b3Cos native_cos\n" +"#define B3_STATIC\n" +"#endif\n" +"#endif\n" +"#ifndef B3_FLOAT4_H\n" +"#define B3_FLOAT4_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif\n" +"#endif\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Float4;\n" +" #define b3Float4ConstArg const b3Float4\n" +" #define b3MakeFloat4 (float4)\n" +" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return dot(a1, b1);\n" +" }\n" +" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return cross(a1, b1);\n" +" }\n" +" #define b3MinFloat4 min\n" +" #define b3MaxFloat4 max\n" +" #define b3Normalized(a) normalize(a)\n" +"#endif \n" +" \n" +"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" +"{\n" +" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" +" return false;\n" +" return true;\n" +"}\n" +"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" +"{\n" +" float maxDot = -B3_INFINITY;\n" +" int i = 0;\n" +" int ptIndex = -1;\n" +" for( i = 0; i < vecLen; i++ )\n" +" {\n" +" float dot = b3Dot3F4(vecArray[i],vec);\n" +" \n" +" if( dot > maxDot )\n" +" {\n" +" maxDot = dot;\n" +" ptIndex = i;\n" +" }\n" +" }\n" +" b3Assert(ptIndex>=0);\n" +" if (ptIndex<0)\n" +" {\n" +" ptIndex = 0;\n" +" }\n" +" *dotOut = maxDot;\n" +" return ptIndex;\n" +"}\n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_RIGIDBODY_DATA_H\n" +"#define B3_RIGIDBODY_DATA_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_QUAT_H\n" +"#define B3_QUAT_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif\n" +"#endif\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Quat;\n" +" #define b3QuatConstArg const b3Quat\n" +" \n" +" \n" +"inline float4 b3FastNormalize4(float4 v)\n" +"{\n" +" v = (float4)(v.xyz,0.f);\n" +" return fast_normalize(v);\n" +"}\n" +" \n" +"inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" +"inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" +"inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" +"{\n" +" b3Quat ans;\n" +" ans = b3Cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - b3Dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" +"{\n" +" b3Quat q;\n" +" q=in;\n" +" //return b3FastNormalize4(in);\n" +" float len = native_sqrt(dot(q, q));\n" +" if(len > 0.f)\n" +" {\n" +" q *= 1.f / len;\n" +" }\n" +" else\n" +" {\n" +" q.x = q.y = q.z = 0.f;\n" +" q.w = 1.f;\n" +" }\n" +" return q;\n" +"}\n" +"inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" b3Quat qInv = b3QuatInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" return b3QuatRotate( b3QuatInvert( q ), vec );\n" +"}\n" +"inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" +"{\n" +" return b3QuatRotate( orientation, point ) + (translation);\n" +"}\n" +" \n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifndef B3_MAT3x3_H\n" +"#define B3_MAT3x3_H\n" +"#ifndef B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"typedef struct\n" +"{\n" +" b3Float4 m_row[3];\n" +"}b3Mat3x3;\n" +"#define b3Mat3x3ConstArg const b3Mat3x3\n" +"#define b3GetRow(m,row) (m.m_row[row])\n" +"inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" +"{\n" +" b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" +" b3Mat3x3 out;\n" +" out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" +" out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" +" out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" +" out.m_row[0].w = 0.f;\n" +" out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" +" out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" +" out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" +" out.m_row[1].w = 0.f;\n" +" out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" +" out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" +" out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" +" out.m_row[2].w = 0.f;\n" +" return out;\n" +"}\n" +"inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = fabs(matIn.m_row[0]);\n" +" out.m_row[1] = fabs(matIn.m_row[1]);\n" +" out.m_row[2] = fabs(matIn.m_row[2]);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtZero();\n" +"__inline\n" +"b3Mat3x3 mtIdentity();\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Mat3x3 mtZero()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(0.f);\n" +" m.m_row[1] = (b3Float4)(0.f);\n" +" m.m_row[2] = (b3Float4)(0.f);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtIdentity()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(1,0,0,0);\n" +" m.m_row[1] = (b3Float4)(0,1,0,0);\n" +" m.m_row[2] = (b3Float4)(0,0,1,0);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" +" out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" +" out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" +"{\n" +" b3Mat3x3 transB;\n" +" transB = mtTranspose( b );\n" +" b3Mat3x3 ans;\n" +" // why this doesn't run when 0ing in the for{}\n" +" a.m_row[0].w = 0.f;\n" +" a.m_row[1].w = 0.f;\n" +" a.m_row[2].w = 0.f;\n" +" for(int i=0; i<3; i++)\n" +" {\n" +"// a.m_row[i].w = 0.f;\n" +" ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" +" ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" +" ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" +" ans.m_row[i].w = 0.f;\n" +" }\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" +"{\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a.m_row[0], b );\n" +" ans.y = b3Dot3F4( a.m_row[1], b );\n" +" ans.z = b3Dot3F4( a.m_row[2], b );\n" +" ans.w = 0.f;\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" +"{\n" +" b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" +" b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" +" b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a, colx );\n" +" ans.y = b3Dot3F4( a, coly );\n" +" ans.z = b3Dot3F4( a, colz );\n" +" return ans;\n" +"}\n" +"#endif\n" +"#endif //B3_MAT3x3_H\n" +"typedef struct b3RigidBodyData b3RigidBodyData_t;\n" +"struct b3RigidBodyData\n" +"{\n" +" b3Float4 m_pos;\n" +" b3Quat m_quat;\n" +" b3Float4 m_linVel;\n" +" b3Float4 m_angVel;\n" +" int m_collidableIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"};\n" +"typedef struct b3InertiaData b3InertiaData_t;\n" +"struct b3InertiaData\n" +"{\n" +" b3Mat3x3 m_invInertiaWorld;\n" +" b3Mat3x3 m_initInvInertia;\n" +"};\n" +"#endif //B3_RIGIDBODY_DATA_H\n" +" \n" +"#ifndef B3_CONVEX_POLYHEDRON_DATA_H\n" +"#define B3_CONVEX_POLYHEDRON_DATA_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"typedef struct b3GpuFace b3GpuFace_t;\n" +"struct b3GpuFace\n" +"{\n" +" b3Float4 m_plane;\n" +" int m_indexOffset;\n" +" int m_numIndices;\n" +" int m_unusedPadding1;\n" +" int m_unusedPadding2;\n" +"};\n" +"typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;\n" +"struct b3ConvexPolyhedronData\n" +"{\n" +" b3Float4 m_localCenter;\n" +" b3Float4 m_extents;\n" +" b3Float4 mC;\n" +" b3Float4 mE;\n" +" float m_radius;\n" +" int m_faceOffset;\n" +" int m_numFaces;\n" +" int m_numVertices;\n" +" int m_vertexOffset;\n" +" int m_uniqueEdgesOffset;\n" +" int m_numUniqueEdges;\n" +" int m_unused;\n" +"};\n" +"#endif //B3_CONVEX_POLYHEDRON_DATA_H\n" +"#ifndef B3_COLLIDABLE_H\n" +"#define B3_COLLIDABLE_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"enum b3ShapeTypes\n" +"{\n" +" SHAPE_HEIGHT_FIELD=1,\n" +" SHAPE_CONVEX_HULL=3,\n" +" SHAPE_PLANE=4,\n" +" SHAPE_CONCAVE_TRIMESH=5,\n" +" SHAPE_COMPOUND_OF_CONVEX_HULLS=6,\n" +" SHAPE_SPHERE=7,\n" +" MAX_NUM_SHAPE_TYPES,\n" +"};\n" +"typedef struct b3Collidable b3Collidable_t;\n" +"struct b3Collidable\n" +"{\n" +" union {\n" +" int m_numChildShapes;\n" +" int m_bvhIndex;\n" +" };\n" +" union\n" +" {\n" +" float m_radius;\n" +" int m_compoundBvhIndex;\n" +" };\n" +" int m_shapeType;\n" +" int m_shapeIndex;\n" +"};\n" +"typedef struct b3GpuChildShape b3GpuChildShape_t;\n" +"struct b3GpuChildShape\n" +"{\n" +" b3Float4 m_childPosition;\n" +" b3Quat m_childOrientation;\n" +" int m_shapeIndex;\n" +" int m_unused0;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"};\n" +"struct b3CompoundOverlappingPair\n" +"{\n" +" int m_bodyIndexA;\n" +" int m_bodyIndexB;\n" +"// int m_pairType;\n" +" int m_childShapeIndexA;\n" +" int m_childShapeIndexB;\n" +"};\n" +"#endif //B3_COLLIDABLE_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#define B3_MPR_SQRT sqrt\n" +"#endif\n" +"#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))\n" +"#define B3_MPR_FABS fabs\n" +"#define B3_MPR_TOLERANCE 1E-6f\n" +"#define B3_MPR_MAX_ITERATIONS 1000\n" +"struct _b3MprSupport_t \n" +"{\n" +" b3Float4 v; //!< Support point in minkowski sum\n" +" b3Float4 v1; //!< Support point in obj1\n" +" b3Float4 v2; //!< Support point in obj2\n" +"};\n" +"typedef struct _b3MprSupport_t b3MprSupport_t;\n" +"struct _b3MprSimplex_t \n" +"{\n" +" b3MprSupport_t ps[4];\n" +" int last; //!< index of last added point\n" +"};\n" +"typedef struct _b3MprSimplex_t b3MprSimplex_t;\n" +"inline b3MprSupport_t* b3MprSimplexPointW(b3MprSimplex_t *s, int idx)\n" +"{\n" +" return &s->ps[idx];\n" +"}\n" +"inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size)\n" +"{\n" +" s->last = size - 1;\n" +"}\n" +"inline int b3MprSimplexSize(const b3MprSimplex_t *s)\n" +"{\n" +" return s->last + 1;\n" +"}\n" +"inline const b3MprSupport_t* b3MprSimplexPoint(const b3MprSimplex_t* s, int idx)\n" +"{\n" +" // here is no check on boundaries\n" +" return &s->ps[idx];\n" +"}\n" +"inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s)\n" +"{\n" +" *d = *s;\n" +"}\n" +"inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a)\n" +"{\n" +" b3MprSupportCopy(s->ps + pos, a);\n" +"}\n" +"inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)\n" +"{\n" +" b3MprSupport_t supp;\n" +" b3MprSupportCopy(&supp, &s->ps[pos1]);\n" +" b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]);\n" +" b3MprSupportCopy(&s->ps[pos2], &supp);\n" +"}\n" +"inline int b3MprIsZero(float val)\n" +"{\n" +" return B3_MPR_FABS(val) < FLT_EPSILON;\n" +"}\n" +"inline int b3MprEq(float _a, float _b)\n" +"{\n" +" float ab;\n" +" float a, b;\n" +" ab = B3_MPR_FABS(_a - _b);\n" +" if (B3_MPR_FABS(ab) < FLT_EPSILON)\n" +" return 1;\n" +" a = B3_MPR_FABS(_a);\n" +" b = B3_MPR_FABS(_b);\n" +" if (b > a){\n" +" return ab < FLT_EPSILON * b;\n" +" }else{\n" +" return ab < FLT_EPSILON * a;\n" +" }\n" +"}\n" +"inline int b3MprVec3Eq(const b3Float4* a, const b3Float4 *b)\n" +"{\n" +" return b3MprEq((*a).x, (*b).x)\n" +" && b3MprEq((*a).y, (*b).y)\n" +" && b3MprEq((*a).z, (*b).z);\n" +"}\n" +"inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global const b3ConvexPolyhedronData_t* hull, b3ConstArray(b3Float4) verticesA)\n" +"{\n" +" b3Float4 supVec = b3MakeFloat4(0,0,0,0);\n" +" float maxDot = -B3_LARGE_FLOAT;\n" +" if( 0 < hull->m_numVertices )\n" +" {\n" +" const b3Float4 scaled = supportVec;\n" +" int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot);\n" +" return verticesA[hull->m_vertexOffset+index];\n" +" }\n" +" return supVec;\n" +"}\n" +"B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" +" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" +" b3ConstArray(b3Collidable_t) cpuCollidables,\n" +" b3ConstArray(b3Float4) cpuVertices,\n" +" __global b3Float4* sepAxis,\n" +" const b3Float4* _dir, b3Float4* outp, int logme)\n" +"{\n" +" //dir is in worldspace, move to local space\n" +" \n" +" b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos;\n" +" b3Quat orn = cpuBodyBuf[bodyIndex].m_quat;\n" +" \n" +" b3Float4 dir = b3MakeFloat4((*_dir).x,(*_dir).y,(*_dir).z,0.f);\n" +" \n" +" const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),dir);\n" +" \n" +" //find local support vertex\n" +" int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx;\n" +" \n" +" b3Assert(cpuCollidables[colIndex].m_shapeType==SHAPE_CONVEX_HULL);\n" +" __global const b3ConvexPolyhedronData_t* hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex];\n" +" \n" +" b3Float4 pInA;\n" +" if (logme)\n" +" {\n" +" b3Float4 supVec = b3MakeFloat4(0,0,0,0);\n" +" float maxDot = -B3_LARGE_FLOAT;\n" +" if( 0 < hull->m_numVertices )\n" +" {\n" +" const b3Float4 scaled = localDir;\n" +" int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot);\n" +" pInA = cpuVertices[hull->m_vertexOffset+index];\n" +" \n" +" }\n" +" } else\n" +" {\n" +" pInA = b3LocalGetSupportVertex(localDir,hull,cpuVertices);\n" +" }\n" +" //move vertex to world space\n" +" *outp = b3TransformPoint(pInA,pos,orn);\n" +" \n" +"}\n" +"inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" +" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" +" b3ConstArray(b3Collidable_t) cpuCollidables,\n" +" b3ConstArray(b3Float4) cpuVertices,\n" +" __global b3Float4* sepAxis,\n" +" const b3Float4* _dir, b3MprSupport_t *supp)\n" +"{\n" +" b3Float4 dir;\n" +" dir = *_dir;\n" +" b3MprConvexSupport(pairIndex,bodyIndexA,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v1,0);\n" +" dir = *_dir*-1.f;\n" +" b3MprConvexSupport(pairIndex,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v2,0);\n" +" supp->v = supp->v1 - supp->v2;\n" +"}\n" +"inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center)\n" +"{\n" +" center->v1 = cpuBodyBuf[bodyIndexA].m_pos;\n" +" center->v2 = cpuBodyBuf[bodyIndexB].m_pos;\n" +" center->v = center->v1 - center->v2;\n" +"}\n" +"inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z)\n" +"{\n" +" (*v).x = x;\n" +" (*v).y = y;\n" +" (*v).z = z;\n" +" (*v).w = 0.f;\n" +"}\n" +"inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w)\n" +"{\n" +" (*v).x += (*w).x;\n" +" (*v).y += (*w).y;\n" +" (*v).z += (*w).z;\n" +"}\n" +"inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w)\n" +"{\n" +" *v = *w;\n" +"}\n" +"inline void b3MprVec3Scale(b3Float4 *d, float k)\n" +"{\n" +" *d *= k;\n" +"}\n" +"inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b)\n" +"{\n" +" float dot;\n" +" dot = b3Dot3F4(*a,*b);\n" +" return dot;\n" +"}\n" +"inline float b3MprVec3Len2(const b3Float4 *v)\n" +"{\n" +" return b3MprVec3Dot(v, v);\n" +"}\n" +"inline void b3MprVec3Normalize(b3Float4 *d)\n" +"{\n" +" float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d));\n" +" b3MprVec3Scale(d, k);\n" +"}\n" +"inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b)\n" +"{\n" +" *d = b3Cross3(*a,*b);\n" +" \n" +"}\n" +"inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)\n" +"{\n" +" *d = *v - *w;\n" +"}\n" +"inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir)\n" +"{\n" +" b3Float4 v2v1, v3v1;\n" +" b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v,\n" +" &b3MprSimplexPoint(portal, 1)->v);\n" +" b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v,\n" +" &b3MprSimplexPoint(portal, 1)->v);\n" +" b3MprVec3Cross(dir, &v2v1, &v3v1);\n" +" b3MprVec3Normalize(dir);\n" +"}\n" +"inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal,\n" +" const b3Float4 *dir)\n" +"{\n" +" float dot;\n" +" dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v);\n" +" return b3MprIsZero(dot) || dot > 0.f;\n" +"}\n" +"inline int portalReachTolerance(const b3MprSimplex_t *portal,\n" +" const b3MprSupport_t *v4,\n" +" const b3Float4 *dir)\n" +"{\n" +" float dv1, dv2, dv3, dv4;\n" +" float dot1, dot2, dot3;\n" +" // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}\n" +" dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir);\n" +" dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir);\n" +" dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir);\n" +" dv4 = b3MprVec3Dot(&v4->v, dir);\n" +" dot1 = dv4 - dv1;\n" +" dot2 = dv4 - dv2;\n" +" dot3 = dv4 - dv3;\n" +" dot1 = B3_MPR_FMIN(dot1, dot2);\n" +" dot1 = B3_MPR_FMIN(dot1, dot3);\n" +" return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE;\n" +"}\n" +"inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal, \n" +" const b3MprSupport_t *v4,\n" +" const b3Float4 *dir)\n" +"{\n" +" float dot;\n" +" dot = b3MprVec3Dot(&v4->v, dir);\n" +" return b3MprIsZero(dot) || dot > 0.f;\n" +"}\n" +"inline void b3ExpandPortal(b3MprSimplex_t *portal,\n" +" const b3MprSupport_t *v4)\n" +"{\n" +" float dot;\n" +" b3Float4 v4v0;\n" +" b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);\n" +" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);\n" +" if (dot > 0.f){\n" +" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);\n" +" if (dot > 0.f){\n" +" b3MprSimplexSet(portal, 1, v4);\n" +" }else{\n" +" b3MprSimplexSet(portal, 3, v4);\n" +" }\n" +" }else{\n" +" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);\n" +" if (dot > 0.f){\n" +" b3MprSimplexSet(portal, 2, v4);\n" +" }else{\n" +" b3MprSimplexSet(portal, 1, v4);\n" +" }\n" +" }\n" +"}\n" +"B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" +" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" +" b3ConstArray(b3Collidable_t) cpuCollidables,\n" +" b3ConstArray(b3Float4) cpuVertices,\n" +" __global b3Float4* sepAxis,\n" +" __global int* hasSepAxis,\n" +" b3MprSimplex_t *portal)\n" +"{\n" +" b3Float4 dir, va, vb;\n" +" float dot;\n" +" int cont;\n" +" \n" +" \n" +" // vertex 0 is center of portal\n" +" b3FindOrigin(bodyIndexA,bodyIndexB,cpuBodyBuf, b3MprSimplexPointW(portal, 0));\n" +" // vertex 0 is center of portal\n" +" b3MprSimplexSetSize(portal, 1);\n" +" \n" +" b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" +" b3Float4* b3mpr_vec3_origin = &zero;\n" +" if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){\n" +" // Portal's center lies on origin (0,0,0) => we know that objects\n" +" // intersect but we would need to know penetration info.\n" +" // So move center little bit...\n" +" b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);\n" +" b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);\n" +" }\n" +" // vertex 1 = support in direction of origin\n" +" b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);\n" +" b3MprVec3Scale(&dir, -1.f);\n" +" b3MprVec3Normalize(&dir);\n" +" b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 1));\n" +" b3MprSimplexSetSize(portal, 2);\n" +" // test if origin isn't outside of v1\n" +" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);\n" +" \n" +" if (b3MprIsZero(dot) || dot < 0.f)\n" +" return -1;\n" +" // vertex 2\n" +" b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,\n" +" &b3MprSimplexPoint(portal, 1)->v);\n" +" if (b3MprIsZero(b3MprVec3Len2(&dir))){\n" +" if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){\n" +" // origin lies on v1\n" +" return 1;\n" +" }else{\n" +" // origin lies on v0-v1 segment\n" +" return 2;\n" +" }\n" +" }\n" +" b3MprVec3Normalize(&dir);\n" +" b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 2));\n" +" \n" +" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir);\n" +" if (b3MprIsZero(dot) || dot < 0.f)\n" +" return -1;\n" +" b3MprSimplexSetSize(portal, 3);\n" +" // vertex 3 direction\n" +" b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,\n" +" &b3MprSimplexPoint(portal, 0)->v);\n" +" b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,\n" +" &b3MprSimplexPoint(portal, 0)->v);\n" +" b3MprVec3Cross(&dir, &va, &vb);\n" +" b3MprVec3Normalize(&dir);\n" +" // it is better to form portal faces to be oriented \"outside\" origin\n" +" dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);\n" +" if (dot > 0.f){\n" +" b3MprSimplexSwap(portal, 1, 2);\n" +" b3MprVec3Scale(&dir, -1.f);\n" +" }\n" +" while (b3MprSimplexSize(portal) < 4){\n" +" b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 3));\n" +" \n" +" dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);\n" +" if (b3MprIsZero(dot) || dot < 0.f)\n" +" return -1;\n" +" cont = 0;\n" +" // test if origin is outside (v1, v0, v3) - set v2 as v3 and\n" +" // continue\n" +" b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,\n" +" &b3MprSimplexPoint(portal, 3)->v);\n" +" dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);\n" +" if (dot < 0.f && !b3MprIsZero(dot)){\n" +" b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));\n" +" cont = 1;\n" +" }\n" +" if (!cont){\n" +" // test if origin is outside (v3, v0, v2) - set v1 as v3 and\n" +" // continue\n" +" b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,\n" +" &b3MprSimplexPoint(portal, 2)->v);\n" +" dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);\n" +" if (dot < 0.f && !b3MprIsZero(dot)){\n" +" b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));\n" +" cont = 1;\n" +" }\n" +" }\n" +" if (cont){\n" +" b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,\n" +" &b3MprSimplexPoint(portal, 0)->v);\n" +" b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,\n" +" &b3MprSimplexPoint(portal, 0)->v);\n" +" b3MprVec3Cross(&dir, &va, &vb);\n" +" b3MprVec3Normalize(&dir);\n" +" }else{\n" +" b3MprSimplexSetSize(portal, 4);\n" +" }\n" +" }\n" +" return 0;\n" +"}\n" +"B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" +" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" +" b3ConstArray(b3Collidable_t) cpuCollidables,\n" +" b3ConstArray(b3Float4) cpuVertices,\n" +" __global b3Float4* sepAxis,\n" +" b3MprSimplex_t *portal)\n" +"{\n" +" b3Float4 dir;\n" +" b3MprSupport_t v4;\n" +" for (int i=0;iv,\n" +" &b3MprSimplexPoint(portal, 2)->v);\n" +" b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);\n" +" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,\n" +" &b3MprSimplexPoint(portal, 2)->v);\n" +" b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);\n" +" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v,\n" +" &b3MprSimplexPoint(portal, 1)->v);\n" +" b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);\n" +" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,\n" +" &b3MprSimplexPoint(portal, 1)->v);\n" +" b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);\n" +" sum = b[0] + b[1] + b[2] + b[3];\n" +" if (b3MprIsZero(sum) || sum < 0.f){\n" +" b[0] = 0.f;\n" +" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,\n" +" &b3MprSimplexPoint(portal, 3)->v);\n" +" b[1] = b3MprVec3Dot(&vec, &dir);\n" +" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,\n" +" &b3MprSimplexPoint(portal, 1)->v);\n" +" b[2] = b3MprVec3Dot(&vec, &dir);\n" +" b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,\n" +" &b3MprSimplexPoint(portal, 2)->v);\n" +" b[3] = b3MprVec3Dot(&vec, &dir);\n" +" sum = b[1] + b[2] + b[3];\n" +" }\n" +" inv = 1.f / sum;\n" +" b3MprVec3Copy(&p1, b3mpr_vec3_origin);\n" +" b3MprVec3Copy(&p2, b3mpr_vec3_origin);\n" +" for (i = 0; i < 4; i++){\n" +" b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);\n" +" b3MprVec3Scale(&vec, b[i]);\n" +" b3MprVec3Add(&p1, &vec);\n" +" b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2);\n" +" b3MprVec3Scale(&vec, b[i]);\n" +" b3MprVec3Add(&p2, &vec);\n" +" }\n" +" b3MprVec3Scale(&p1, inv);\n" +" b3MprVec3Scale(&p2, inv);\n" +" b3MprVec3Copy(pos, &p1);\n" +" b3MprVec3Add(pos, &p2);\n" +" b3MprVec3Scale(pos, 0.5);\n" +"}\n" +"inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b)\n" +"{\n" +" b3Float4 ab;\n" +" b3MprVec3Sub2(&ab, a, b);\n" +" return b3MprVec3Len2(&ab);\n" +"}\n" +"inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,\n" +" const b3Float4 *x0,\n" +" const b3Float4 *b,\n" +" b3Float4 *witness)\n" +"{\n" +" // The computation comes from solving equation of segment:\n" +" // S(t) = x0 + t.d\n" +" // where - x0 is initial point of segment\n" +" // - d is direction of segment from x0 (|d| > 0)\n" +" // - t belongs to <0, 1> interval\n" +" // \n" +" // Than, distance from a segment to some point P can be expressed:\n" +" // D(t) = |x0 + t.d - P|^2\n" +" // which is distance from any point on segment. Minimization\n" +" // of this function brings distance from P to segment.\n" +" // Minimization of D(t) leads to simple quadratic equation that's\n" +" // solving is straightforward.\n" +" //\n" +" // Bonus of this method is witness point for free.\n" +" float dist, t;\n" +" b3Float4 d, a;\n" +" // direction of segment\n" +" b3MprVec3Sub2(&d, b, x0);\n" +" // precompute vector from P to x0\n" +" b3MprVec3Sub2(&a, x0, P);\n" +" t = -1.f * b3MprVec3Dot(&a, &d);\n" +" t /= b3MprVec3Len2(&d);\n" +" if (t < 0.f || b3MprIsZero(t)){\n" +" dist = b3MprVec3Dist2(x0, P);\n" +" if (witness)\n" +" b3MprVec3Copy(witness, x0);\n" +" }else if (t > 1.f || b3MprEq(t, 1.f)){\n" +" dist = b3MprVec3Dist2(b, P);\n" +" if (witness)\n" +" b3MprVec3Copy(witness, b);\n" +" }else{\n" +" if (witness){\n" +" b3MprVec3Copy(witness, &d);\n" +" b3MprVec3Scale(witness, t);\n" +" b3MprVec3Add(witness, x0);\n" +" dist = b3MprVec3Dist2(witness, P);\n" +" }else{\n" +" // recycling variables\n" +" b3MprVec3Scale(&d, t);\n" +" b3MprVec3Add(&d, &a);\n" +" dist = b3MprVec3Len2(&d);\n" +" }\n" +" }\n" +" return dist;\n" +"}\n" +"inline float b3MprVec3PointTriDist2(const b3Float4 *P,\n" +" const b3Float4 *x0, const b3Float4 *B,\n" +" const b3Float4 *C,\n" +" b3Float4 *witness)\n" +"{\n" +" // Computation comes from analytic expression for triangle (x0, B, C)\n" +" // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and\n" +" // Then equation for distance is:\n" +" // D(s, t) = | T(s, t) - P |^2\n" +" // This leads to minimization of quadratic function of two variables.\n" +" // The solution from is taken only if s is between 0 and 1, t is\n" +" // between 0 and 1 and t + s < 1, otherwise distance from segment is\n" +" // computed.\n" +" b3Float4 d1, d2, a;\n" +" float u, v, w, p, q, r;\n" +" float s, t, dist, dist2;\n" +" b3Float4 witness2;\n" +" b3MprVec3Sub2(&d1, B, x0);\n" +" b3MprVec3Sub2(&d2, C, x0);\n" +" b3MprVec3Sub2(&a, x0, P);\n" +" u = b3MprVec3Dot(&a, &a);\n" +" v = b3MprVec3Dot(&d1, &d1);\n" +" w = b3MprVec3Dot(&d2, &d2);\n" +" p = b3MprVec3Dot(&a, &d1);\n" +" q = b3MprVec3Dot(&a, &d2);\n" +" r = b3MprVec3Dot(&d1, &d2);\n" +" s = (q * r - w * p) / (w * v - r * r);\n" +" t = (-s * r - q) / w;\n" +" if ((b3MprIsZero(s) || s > 0.f)\n" +" && (b3MprEq(s, 1.f) || s < 1.f)\n" +" && (b3MprIsZero(t) || t > 0.f)\n" +" && (b3MprEq(t, 1.f) || t < 1.f)\n" +" && (b3MprEq(t + s, 1.f) || t + s < 1.f)){\n" +" if (witness){\n" +" b3MprVec3Scale(&d1, s);\n" +" b3MprVec3Scale(&d2, t);\n" +" b3MprVec3Copy(witness, x0);\n" +" b3MprVec3Add(witness, &d1);\n" +" b3MprVec3Add(witness, &d2);\n" +" dist = b3MprVec3Dist2(witness, P);\n" +" }else{\n" +" dist = s * s * v;\n" +" dist += t * t * w;\n" +" dist += 2.f * s * t * r;\n" +" dist += 2.f * s * p;\n" +" dist += 2.f * t * q;\n" +" dist += u;\n" +" }\n" +" }else{\n" +" dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);\n" +" dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);\n" +" if (dist2 < dist){\n" +" dist = dist2;\n" +" if (witness)\n" +" b3MprVec3Copy(witness, &witness2);\n" +" }\n" +" dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);\n" +" if (dist2 < dist){\n" +" dist = dist2;\n" +" if (witness)\n" +" b3MprVec3Copy(witness, &witness2);\n" +" }\n" +" }\n" +" return dist;\n" +"}\n" +"B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, \n" +" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" +" b3ConstArray(b3Collidable_t) cpuCollidables,\n" +" b3ConstArray(b3Float4) cpuVertices,\n" +" __global b3Float4* sepAxis,\n" +" b3MprSimplex_t *portal,\n" +" float *depth, b3Float4 *pdir, b3Float4 *pos)\n" +"{\n" +" b3Float4 dir;\n" +" b3MprSupport_t v4;\n" +" unsigned long iterations;\n" +" b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" +" b3Float4* b3mpr_vec3_origin = &zero;\n" +" iterations = 1UL;\n" +" for (int i=0;i find penetration info\n" +" if (portalReachTolerance(portal, &v4, &dir)\n" +" || iterations ==B3_MPR_MAX_ITERATIONS)\n" +" {\n" +" *depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin,&b3MprSimplexPoint(portal, 1)->v,&b3MprSimplexPoint(portal, 2)->v,&b3MprSimplexPoint(portal, 3)->v,pdir);\n" +" *depth = B3_MPR_SQRT(*depth);\n" +" \n" +" if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z))\n" +" {\n" +" \n" +" *pdir = dir;\n" +" } \n" +" b3MprVec3Normalize(pdir);\n" +" \n" +" // barycentric coordinates:\n" +" b3FindPos(portal, pos);\n" +" return;\n" +" }\n" +" b3ExpandPortal(portal, &v4);\n" +" iterations++;\n" +" }\n" +"}\n" +"B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos)\n" +"{\n" +" // Touching contact on portal's v1 - so depth is zero and direction\n" +" // is unimportant and pos can be guessed\n" +" *depth = 0.f;\n" +" b3Float4 zero = b3MakeFloat4(0,0,0,0);\n" +" b3Float4* b3mpr_vec3_origin = &zero;\n" +" b3MprVec3Copy(dir, b3mpr_vec3_origin);\n" +" b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);\n" +" b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);\n" +" b3MprVec3Scale(pos, 0.5);\n" +"}\n" +"B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,\n" +" float *depth, b3Float4 *dir, b3Float4 *pos)\n" +"{\n" +" \n" +" // Origin lies on v0-v1 segment.\n" +" // Depth is distance to v1, direction also and position must be\n" +" // computed\n" +" b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);\n" +" b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);\n" +" b3MprVec3Scale(pos, 0.5f);\n" +" \n" +" b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);\n" +" *depth = B3_MPR_SQRT(b3MprVec3Len2(dir));\n" +" b3MprVec3Normalize(dir);\n" +"}\n" +"inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,\n" +" b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,\n" +" b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, \n" +" b3ConstArray(b3Collidable_t) cpuCollidables,\n" +" b3ConstArray(b3Float4) cpuVertices,\n" +" __global b3Float4* sepAxis,\n" +" __global int* hasSepAxis,\n" +" float *depthOut, b3Float4* dirOut, b3Float4* posOut)\n" +"{\n" +" \n" +" b3MprSimplex_t portal;\n" +" \n" +"// if (!hasSepAxis[pairIndex])\n" +" // return -1;\n" +" \n" +" hasSepAxis[pairIndex] = 0;\n" +" int res;\n" +" // Phase 1: Portal discovery\n" +" res = b3DiscoverPortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,hasSepAxis, &portal);\n" +" \n" +" \n" +" //sepAxis[pairIndex] = *pdir;//or -dir?\n" +" switch (res)\n" +" {\n" +" case 0:\n" +" {\n" +" // Phase 2: Portal refinement\n" +" \n" +" res = b3RefinePortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal);\n" +" if (res < 0)\n" +" return -1;\n" +" // Phase 3. Penetration info\n" +" b3FindPenetr(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal, depthOut, dirOut, posOut);\n" +" hasSepAxis[pairIndex] = 1;\n" +" sepAxis[pairIndex] = -*dirOut;\n" +" break;\n" +" }\n" +" case 1:\n" +" {\n" +" // Touching contact on portal's v1.\n" +" b3FindPenetrTouch(&portal, depthOut, dirOut, posOut);\n" +" break;\n" +" }\n" +" case 2:\n" +" {\n" +" \n" +" b3FindPenetrSegment( &portal, depthOut, dirOut, posOut);\n" +" break;\n" +" }\n" +" default:\n" +" {\n" +" hasSepAxis[pairIndex]=0;\n" +" //if (res < 0)\n" +" //{\n" +" // Origin isn't inside portal - no collision.\n" +" return -1;\n" +" //}\n" +" }\n" +" };\n" +" \n" +" return 0;\n" +"};\n" +"#endif //B3_MPR_PENETRATION_H\n" +"#ifndef B3_CONTACT4DATA_H\n" +"#define B3_CONTACT4DATA_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"typedef struct b3Contact4Data b3Contact4Data_t;\n" +"struct b3Contact4Data\n" +"{\n" +" b3Float4 m_worldPosB[4];\n" +"// b3Float4 m_localPosA[4];\n" +"// b3Float4 m_localPosB[4];\n" +" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" +" unsigned short m_restituitionCoeffCmp;\n" +" unsigned short m_frictionCoeffCmp;\n" +" int m_batchIdx;\n" +" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" +" int m_bodyBPtrAndSignBit;\n" +" int m_childIndexA;\n" +" int m_childIndexB;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"};\n" +"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" +"{\n" +" return (int)contact->m_worldNormalOnB.w;\n" +"};\n" +"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" +"{\n" +" contact->m_worldNormalOnB.w = (float)numPoints;\n" +"};\n" +"#endif //B3_CONTACT4DATA_H\n" +"#define AppendInc(x, out) out = atomic_inc(x)\n" +"#define GET_NPOINTS(x) (x).m_worldNormalOnB.w\n" +"#ifdef cl_ext_atomic_counters_32\n" +" #pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" +"#else\n" +" #define counter32_t volatile __global int*\n" +"#endif\n" +"__kernel void mprPenetrationKernel( __global int4* pairs,\n" +" __global const b3RigidBodyData_t* rigidBodies, \n" +" __global const b3Collidable_t* collidables,\n" +" __global const b3ConvexPolyhedronData_t* convexShapes, \n" +" __global const float4* vertices,\n" +" __global float4* separatingNormals,\n" +" __global int* hasSeparatingAxis,\n" +" __global struct b3Contact4Data* restrict globalContactsOut,\n" +" counter32_t nGlobalContactsOut,\n" +" int contactCapacity,\n" +" int numPairs)\n" +"{\n" +" int i = get_global_id(0);\n" +" int pairIndex = i;\n" +" if (im_worldNormalOnB = -dirOut;//normal;\n" +" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" +" c->m_batchIdx = pairIndex;\n" +" int bodyA = pairs[pairIndex].x;\n" +" int bodyB = pairs[pairIndex].y;\n" +" c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0 ? -bodyA:bodyA;\n" +" c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0 ? -bodyB:bodyB;\n" +" c->m_childIndexA = -1;\n" +" c->m_childIndexB = -1;\n" +" //for (int i=0;im_worldPosB[0] = posOut;//localPoints[contactIdx[i]];\n" +" GET_NPOINTS(*c) = 1;//nContacts;\n" +" }\n" +" }\n" +" }\n" +"}\n" +"typedef float4 Quaternion;\n" +"#define make_float4 (float4)\n" +"__inline\n" +"float dot3F4(float4 a, float4 b)\n" +"{\n" +" float4 a1 = make_float4(a.xyz,0.f);\n" +" float4 b1 = make_float4(b.xyz,0.f);\n" +" return dot(a1, b1);\n" +"}\n" +"__inline\n" +"float4 cross3(float4 a, float4 b)\n" +"{\n" +" return cross(a,b);\n" +"}\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b)\n" +"{\n" +" Quaternion ans;\n" +" ans = cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q)\n" +"{\n" +" return (Quaternion)(-q.xyz, q.w);\n" +"}\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec)\n" +"{\n" +" Quaternion qInv = qtInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"__inline\n" +"float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)\n" +"{\n" +" return qtRotate( *orientation, *p ) + (*translation);\n" +"}\n" +"__inline\n" +"float4 qtInvRotate(const Quaternion q, float4 vec)\n" +"{\n" +" return qtRotate( qtInvert( q ), vec );\n" +"}\n" +"inline void project(__global const b3ConvexPolyhedronData_t* hull, const float4 pos, const float4 orn, \n" +"const float4* dir, __global const float4* vertices, float* min, float* max)\n" +"{\n" +" min[0] = FLT_MAX;\n" +" max[0] = -FLT_MAX;\n" +" int numVerts = hull->m_numVertices;\n" +" const float4 localDir = qtInvRotate(orn,*dir);\n" +" float offset = dot(pos,*dir);\n" +" for(int i=0;im_vertexOffset+i],localDir);\n" +" if(dp < min[0]) \n" +" min[0] = dp;\n" +" if(dp > max[0]) \n" +" max[0] = dp;\n" +" }\n" +" if(min[0]>max[0])\n" +" {\n" +" float tmp = min[0];\n" +" min[0] = max[0];\n" +" max[0] = tmp;\n" +" }\n" +" min[0] += offset;\n" +" max[0] += offset;\n" +"}\n" +"bool findSeparatingAxisUnitSphere( __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB, \n" +" const float4 posA1,\n" +" const float4 ornA,\n" +" const float4 posB1,\n" +" const float4 ornB,\n" +" const float4 DeltaC2,\n" +" __global const float4* vertices,\n" +" __global const float4* unitSphereDirections,\n" +" int numUnitSphereDirections,\n" +" float4* sep,\n" +" float* dmin)\n" +"{\n" +" \n" +" float4 posA = posA1;\n" +" posA.w = 0.f;\n" +" float4 posB = posB1;\n" +" posB.w = 0.f;\n" +" int curPlaneTests=0;\n" +" int curEdgeEdge = 0;\n" +" // Test unit sphere directions\n" +" for (int i=0;i0)\n" +" crossje *= -1.f;\n" +" {\n" +" float dist;\n" +" bool result = true;\n" +" float Min0,Max0;\n" +" float Min1,Max1;\n" +" project(hullA,posA,ornA,&crossje,vertices, &Min0, &Max0);\n" +" project(hullB,posB,ornB,&crossje,vertices, &Min1, &Max1);\n" +" \n" +" if(Max00.0f)\n" +" {\n" +" *sep = -(*sep);\n" +" }\n" +" return true;\n" +"}\n" +"__kernel void findSeparatingAxisUnitSphereKernel( __global const int4* pairs, \n" +" __global const b3RigidBodyData_t* rigidBodies, \n" +" __global const b3Collidable_t* collidables,\n" +" __global const b3ConvexPolyhedronData_t* convexShapes, \n" +" __global const float4* vertices,\n" +" __global const float4* unitSphereDirections,\n" +" __global float4* separatingNormals,\n" +" __global int* hasSeparatingAxis,\n" +" __global float* dmins,\n" +" int numUnitSphereDirections,\n" +" int numPairs\n" +" )\n" +"{\n" +" int i = get_global_id(0);\n" +" \n" +" if (inumUnitSphereDirections)\n" +" {\n" +" bool sepEE = findSeparatingAxisUnitSphere( &convexShapes[shapeIndexA], &convexShapes[shapeIndexB],posA,ornA,\n" +" posB,ornB,\n" +" DeltaC2,\n" +" vertices,unitSphereDirections,numUnitSphereDirections,&sepNormal,&dmin);\n" +" if (!sepEE)\n" +" {\n" +" hasSeparatingAxis[i] = 0;\n" +" } else\n" +" {\n" +" hasSeparatingAxis[i] = 1;\n" +" separatingNormals[i] = sepNormal;\n" +" }\n" +" }\n" +" } //if (hasSeparatingAxis[i])\n" +" }//(im_plane.x,face->m_plane.y,face->m_plane.z,0.f); + + if (face->m_numIndices<2) + return false; + + + float4 v0 = baseVertex[convexIndices[face->m_indexOffset + face->m_numIndices-1]]; + + b = v0; + + for(unsigned i=0; i != face->m_numIndices; ++i) + { + a = b; + float4 vi = baseVertex[convexIndices[face->m_indexOffset + i]]; + b = vi; + ab = b-a; + ap = p-a; + v = cross3(ab,plane); + + if (dot(ap, v) > 0.f) + { + float ab_m2 = dot(ab, ab); + float rt = ab_m2 != 0.f ? dot(ab, ap) / ab_m2 : 0.f; + if (rt <= 0.f) + { + *out = a; + } + else if (rt >= 1.f) + { + *out = b; + } + else + { + float s = 1.f - rt; + out[0].x = s * a.x + rt * b.x; + out[0].y = s * a.y + rt * b.y; + out[0].z = s * a.z + rt * b.z; + } + return false; + } + } + return true; +} + + + + +void computeContactSphereConvex(int pairIndex, + int bodyIndexA, int bodyIndexB, + int collidableIndexA, int collidableIndexB, + __global const BodyData* rigidBodies, + __global const btCollidableGpu* collidables, + __global const ConvexPolyhedronCL* convexShapes, + __global const float4* convexVertices, + __global const int* convexIndices, + __global const btGpuFace* faces, + __global struct b3Contact4Data* restrict globalContactsOut, + counter32_t nGlobalContactsOut, + int maxContactCapacity, + float4 spherePos2, + float radius, + float4 pos, + float4 quat + ) +{ + + float4 invPos; + float4 invOrn; + + trInverse(pos,quat, &invPos,&invOrn); + + float4 spherePos = transform(&spherePos2,&invPos,&invOrn); + + int shapeIndex = collidables[collidableIndexB].m_shapeIndex; + int numFaces = convexShapes[shapeIndex].m_numFaces; + float4 closestPnt = (float4)(0, 0, 0, 0); + float4 hitNormalWorld = (float4)(0, 0, 0, 0); + float minDist = -1000000.f; + bool bCollide = true; + + for ( int f = 0; f < numFaces; f++ ) + { + btGpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f]; + + // set up a plane equation + float4 planeEqn; + float4 n1 = face.m_plane; + n1.w = 0.f; + planeEqn = n1; + planeEqn.w = face.m_plane.w; + + + // compute a signed distance from the vertex in cloth to the face of rigidbody. + float4 pntReturn; + float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn); + + // If the distance is positive, the plane is a separating plane. + if ( dist > radius ) + { + bCollide = false; + break; + } + + + if (dist>0) + { + //might hit an edge or vertex + float4 out; + float4 zeroPos = make_float4(0,0,0,0); + + bool isInPoly = IsPointInPolygon(spherePos, + &face, + &convexVertices[convexShapes[shapeIndex].m_vertexOffset], + convexIndices, + &out); + if (isInPoly) + { + if (dist>minDist) + { + minDist = dist; + closestPnt = pntReturn; + hitNormalWorld = planeEqn; + + } + } else + { + float4 tmp = spherePos-out; + float l2 = dot(tmp,tmp); + if (l2minDist) + { + minDist = dist; + closestPnt = out; + hitNormalWorld = tmp/dist; + + } + + } else + { + bCollide = false; + break; + } + } + } else + { + if ( dist > minDist ) + { + minDist = dist; + closestPnt = pntReturn; + hitNormalWorld.xyz = planeEqn.xyz; + } + } + + } + + + + if (bCollide && minDist > -10000) + { + float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld); + float4 pOnB1 = transform(&closestPnt,&pos,&quat); + + float actualDepth = minDist-radius; + if (actualDepth<=0.f) + { + + + pOnB1.w = actualDepth; + + int dstIdx; + AppendInc( nGlobalContactsOut, dstIdx ); + + + if (1)//dstIdx < maxContactCapacity) + { + __global struct b3Contact4Data* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = -normalOnSurfaceB1; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB; + c->m_worldPosB[0] = pOnB1; + c->m_childIndexA = -1; + c->m_childIndexB = -1; + + GET_NPOINTS(*c) = 1; + } + + } + }//if (hasCollision) + +} + + + +int extractManifoldSequential(const float4* p, int nPoints, float4 nearNormal, int4* contactIdx) +{ + if( nPoints == 0 ) + return 0; + + if (nPoints <=4) + return nPoints; + + + if (nPoints >64) + nPoints = 64; + + float4 center = make_float4(0.f); + { + + for (int i=0;im_numVertices;i++) + { + float4 vtx = convexVertices[hullB->m_vertexOffset+i]; + float curDot = dot(vtx,planeNormalInConvex); + + + if (curDot>maxDot) + { + hitVertex=i; + maxDot=curDot; + hitVtx = vtx; + //make sure the deepest points is always included + if (numPoints==MAX_PLANE_CONVEX_POINTS) + numPoints--; + } + + if (numPoints4) + { + numReducedPoints = extractManifoldSequential( contactPoints, numPoints, planeNormalInConvex, &contactIdx); + } + + if (numReducedPoints>0) + { + int dstIdx; + AppendInc( nGlobalContactsOut, dstIdx ); + + if (dstIdx < maxContactCapacity) + { + resultIndex = dstIdx; + __global struct b3Contact4Data* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = -planeNormalWorld; + //c->setFrictionCoeff(0.7); + //c->setRestituitionCoeff(0.f); + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB; + c->m_childIndexA = -1; + c->m_childIndexB = -1; + + switch (numReducedPoints) + { + case 4: + c->m_worldPosB[3] = contactPoints[contactIdx.w]; + case 3: + c->m_worldPosB[2] = contactPoints[contactIdx.z]; + case 2: + c->m_worldPosB[1] = contactPoints[contactIdx.y]; + case 1: + c->m_worldPosB[0] = contactPoints[contactIdx.x]; + default: + { + } + }; + + GET_NPOINTS(*c) = numReducedPoints; + }//if (dstIdx < numPairs) + } + + return resultIndex; +} + + +void computeContactPlaneSphere(int pairIndex, + int bodyIndexA, int bodyIndexB, + int collidableIndexA, int collidableIndexB, + __global const BodyData* rigidBodies, + __global const btCollidableGpu* collidables, + __global const btGpuFace* faces, + __global struct b3Contact4Data* restrict globalContactsOut, + counter32_t nGlobalContactsOut, + int maxContactCapacity) +{ + float4 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane; + float radius = collidables[collidableIndexB].m_radius; + float4 posA1 = rigidBodies[bodyIndexA].m_pos; + float4 ornA1 = rigidBodies[bodyIndexA].m_quat; + float4 posB1 = rigidBodies[bodyIndexB].m_pos; + float4 ornB1 = rigidBodies[bodyIndexB].m_quat; + + bool hasCollision = false; + float4 planeNormal1 = make_float4(planeEq.x,planeEq.y,planeEq.z,0.f); + float planeConstant = planeEq.w; + float4 convexInPlaneTransPos1; Quaternion convexInPlaneTransOrn1; + { + float4 invPosA;Quaternion invOrnA; + trInverse(posA1,ornA1,&invPosA,&invOrnA); + trMul(invPosA,invOrnA,posB1,ornB1,&convexInPlaneTransPos1,&convexInPlaneTransOrn1); + } + float4 planeInConvexPos1; Quaternion planeInConvexOrn1; + { + float4 invPosB;Quaternion invOrnB; + trInverse(posB1,ornB1,&invPosB,&invOrnB); + trMul(invPosB,invOrnB,posA1,ornA1,&planeInConvexPos1,&planeInConvexOrn1); + } + float4 vtx1 = qtRotate(planeInConvexOrn1,-planeNormal1)*radius; + float4 vtxInPlane1 = transform(&vtx1,&convexInPlaneTransPos1,&convexInPlaneTransOrn1); + float distance = dot3F4(planeNormal1,vtxInPlane1) - planeConstant; + hasCollision = distance < 0.f;//m_manifoldPtr->getContactBreakingThreshold(); + if (hasCollision) + { + float4 vtxInPlaneProjected1 = vtxInPlane1 - distance*planeNormal1; + float4 vtxInPlaneWorld1 = transform(&vtxInPlaneProjected1,&posA1,&ornA1); + float4 normalOnSurfaceB1 = qtRotate(ornA1,planeNormal1); + float4 pOnB1 = vtxInPlaneWorld1+normalOnSurfaceB1*distance; + pOnB1.w = distance; + + int dstIdx; + AppendInc( nGlobalContactsOut, dstIdx ); + + if (dstIdx < maxContactCapacity) + { + __global struct b3Contact4Data* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = -normalOnSurfaceB1; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB; + c->m_worldPosB[0] = pOnB1; + c->m_childIndexA = -1; + c->m_childIndexB = -1; + GET_NPOINTS(*c) = 1; + }//if (dstIdx < numPairs) + }//if (hasCollision) +} + + +__kernel void primitiveContactsKernel( __global int4* pairs, + __global const BodyData* rigidBodies, + __global const btCollidableGpu* collidables, + __global const ConvexPolyhedronCL* convexShapes, + __global const float4* vertices, + __global const float4* uniqueEdges, + __global const btGpuFace* faces, + __global const int* indices, + __global struct b3Contact4Data* restrict globalContactsOut, + counter32_t nGlobalContactsOut, + int numPairs, int maxContactCapacity) +{ + + int i = get_global_id(0); + int pairIndex = i; + + float4 worldVertsB1[64]; + float4 worldVertsB2[64]; + int capacityWorldVerts = 64; + + float4 localContactsOut[64]; + int localContactCapacity=64; + + float minDist = -1e30f; + float maxDist = 0.02f; + + if (i=0) + pairs[pairIndex].z = contactIndex; + + return; + } + + + if (collidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + collidables[collidableIndexB].m_shapeType == SHAPE_PLANE) + { + + float4 posA; + posA = rigidBodies[bodyIndexA].m_pos; + Quaternion ornA; + ornA = rigidBodies[bodyIndexA].m_quat; + + + int contactIndex = computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, + rigidBodies,collidables,convexShapes,vertices,indices, + faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA); + + if (contactIndex>=0) + pairs[pairIndex].z = contactIndex; + + return; + } + + if (collidables[collidableIndexA].m_shapeType == SHAPE_PLANE && + collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE) + { + computeContactPlaneSphere(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, + rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity); + return; + } + + + if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE && + collidables[collidableIndexB].m_shapeType == SHAPE_PLANE) + { + + + computeContactPlaneSphere( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, + rigidBodies,collidables, + faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity); + + return; + } + + + + + if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE && + collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL) + { + + float4 spherePos = rigidBodies[bodyIndexA].m_pos; + float sphereRadius = collidables[collidableIndexA].m_radius; + float4 convexPos = rigidBodies[bodyIndexB].m_pos; + float4 convexOrn = rigidBodies[bodyIndexB].m_quat; + + computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, + rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity, + spherePos,sphereRadius,convexPos,convexOrn); + + return; + } + + if (collidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL && + collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE) + { + + float4 spherePos = rigidBodies[bodyIndexB].m_pos; + float sphereRadius = collidables[collidableIndexB].m_radius; + float4 convexPos = rigidBodies[bodyIndexA].m_pos; + float4 convexOrn = rigidBodies[bodyIndexA].m_quat; + + computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, + rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity, + spherePos,sphereRadius,convexPos,convexOrn); + return; + } + + + + + + + if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE && + collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE) + { + //sphere-sphere + float radiusA = collidables[collidableIndexA].m_radius; + float radiusB = collidables[collidableIndexB].m_radius; + float4 posA = rigidBodies[bodyIndexA].m_pos; + float4 posB = rigidBodies[bodyIndexB].m_pos; + + float4 diff = posA-posB; + float len = length(diff); + + ///iff distance positive, don't generate a new contact + if ( len <= (radiusA+radiusB)) + { + ///distance (negative means penetration) + float dist = len - (radiusA+radiusB); + float4 normalOnSurfaceB = make_float4(1.f,0.f,0.f,0.f); + if (len > 0.00001) + { + normalOnSurfaceB = diff / len; + } + float4 contactPosB = posB + normalOnSurfaceB*radiusB; + contactPosB.w = dist; + + int dstIdx; + AppendInc( nGlobalContactsOut, dstIdx ); + + if (dstIdx < maxContactCapacity) + { + __global struct b3Contact4Data* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = normalOnSurfaceB; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + int bodyA = pairs[pairIndex].x; + int bodyB = pairs[pairIndex].y; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB; + c->m_worldPosB[0] = contactPosB; + c->m_childIndexA = -1; + c->m_childIndexB = -1; + GET_NPOINTS(*c) = 1; + }//if (dstIdx < numPairs) + }//if ( len <= (radiusA+radiusB)) + + return; + }//SHAPE_SPHERE SHAPE_SPHERE + + }// if (i= 0) + { + collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex; + float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition; + float4 childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation; + float4 newPosA = qtRotate(ornA,childPosA)+posA; + float4 newOrnA = qtMul(ornA,childOrnA); + posA = newPosA; + ornA = newOrnA; + } else + { + collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + } + + if (childShapeIndexB>=0) + { + collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex; + float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition; + float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation; + float4 newPosB = transform(&childPosB,&posB,&ornB); + float4 newOrnB = qtMul(ornB,childOrnB); + posB = newPosB; + ornB = newOrnB; + } else + { + collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + } + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + int shapeTypeA = collidables[collidableIndexA].m_shapeType; + int shapeTypeB = collidables[collidableIndexB].m_shapeType; + + int pairIndex = i; + if ((shapeTypeA == SHAPE_PLANE) && (shapeTypeB==SHAPE_CONVEX_HULL)) + { + + computeContactPlaneConvex( pairIndex, bodyIndexA,bodyIndexB, collidableIndexA,collidableIndexB, + rigidBodies,collidables,convexShapes,vertices,indices, + faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posB,ornB); + return; + } + + if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB==SHAPE_PLANE)) + { + + computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, + rigidBodies,collidables,convexShapes,vertices,indices, + faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA); + return; + } + + if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB == SHAPE_SPHERE)) + { + float4 spherePos = rigidBodies[bodyIndexB].m_pos; + float sphereRadius = collidables[collidableIndexB].m_radius; + float4 convexPos = posA; + float4 convexOrn = ornA; + + computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA , collidableIndexB,collidableIndexA, + rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity, + spherePos,sphereRadius,convexPos,convexOrn); + + return; + } + + if ((shapeTypeA == SHAPE_SPHERE) && (shapeTypeB == SHAPE_CONVEX_HULL)) + { + + float4 spherePos = rigidBodies[bodyIndexA].m_pos; + float sphereRadius = collidables[collidableIndexA].m_radius; + float4 convexPos = posB; + float4 convexOrn = ornB; + + + computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, + rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity, + spherePos,sphereRadius,convexPos,convexOrn); + + return; + } + }// if (i 0 && r2 > 0 && r3 > 0 ) + return true; + if ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) + return true; + return false; + +} + + +float segmentSqrDistance(float4 from, float4 to,float4 p, float4* nearest) +{ + float4 diff = p - from; + float4 v = to - from; + float t = dot(v,diff); + + if (t > 0) + { + float dotVV = dot(v,v); + if (t < dotVV) + { + t /= dotVV; + diff -= t*v; + } else + { + t = 1; + diff -= v; + } + } else + { + t = 0; + } + *nearest = from + t*v; + return dot(diff,diff); +} + + +void computeContactSphereTriangle(int pairIndex, + int bodyIndexA, int bodyIndexB, + int collidableIndexA, int collidableIndexB, + __global const BodyData* rigidBodies, + __global const btCollidableGpu* collidables, + const float4* triangleVertices, + __global struct b3Contact4Data* restrict globalContactsOut, + counter32_t nGlobalContactsOut, + int maxContactCapacity, + float4 spherePos2, + float radius, + float4 pos, + float4 quat, + int faceIndex + ) +{ + + float4 invPos; + float4 invOrn; + + trInverse(pos,quat, &invPos,&invOrn); + float4 spherePos = transform(&spherePos2,&invPos,&invOrn); + int numFaces = 3; + float4 closestPnt = (float4)(0, 0, 0, 0); + float4 hitNormalWorld = (float4)(0, 0, 0, 0); + float minDist = -1000000.f; + bool bCollide = false; + + + ////////////////////////////////////// + + float4 sphereCenter; + sphereCenter = spherePos; + + const float4* vertices = triangleVertices; + float contactBreakingThreshold = 0.f;//todo? + float radiusWithThreshold = radius + contactBreakingThreshold; + float4 edge10; + edge10 = vertices[1]-vertices[0]; + edge10.w = 0.f;//is this needed? + float4 edge20; + edge20 = vertices[2]-vertices[0]; + edge20.w = 0.f;//is this needed? + float4 normal = cross3(edge10,edge20); + normal = normalize(normal); + float4 p1ToCenter; + p1ToCenter = sphereCenter - vertices[0]; + + float distanceFromPlane = dot(p1ToCenter,normal); + + if (distanceFromPlane < 0.f) + { + //triangle facing the other way + distanceFromPlane *= -1.f; + normal *= -1.f; + } + hitNormalWorld = normal; + + bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; + + // Check for contact / intersection + bool hasContact = false; + float4 contactPoint; + if (isInsideContactPlane) + { + + if (pointInTriangle(vertices,&normal, &sphereCenter)) + { + // Inside the contact wedge - touches a point on the shell plane + hasContact = true; + contactPoint = sphereCenter - normal*distanceFromPlane; + + } else { + // Could be inside one of the contact capsules + float contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold; + float4 nearestOnEdge; + int numEdges = 3; + for (int i = 0; i < numEdges; i++) + { + float4 pa =vertices[i]; + float4 pb = vertices[(i+1)%3]; + + float distanceSqr = segmentSqrDistance(pa,pb,sphereCenter, &nearestOnEdge); + if (distanceSqr < contactCapsuleRadiusSqr) + { + // Yep, we're inside a capsule + hasContact = true; + contactPoint = nearestOnEdge; + + } + + } + } + } + + if (hasContact) + { + + closestPnt = contactPoint; + float4 contactToCenter = sphereCenter - contactPoint; + minDist = length(contactToCenter); + if (minDist>FLT_EPSILON) + { + hitNormalWorld = normalize(contactToCenter);//*(1./minDist); + bCollide = true; + } + + } + + + ///////////////////////////////////// + + if (bCollide && minDist > -10000) + { + + float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld); + float4 pOnB1 = transform(&closestPnt,&pos,&quat); + float actualDepth = minDist-radius; + + + if (actualDepth<=0.f) + { + pOnB1.w = actualDepth; + int dstIdx; + + + float lenSqr = dot3F4(normalOnSurfaceB1,normalOnSurfaceB1); + if (lenSqr>FLT_EPSILON) + { + AppendInc( nGlobalContactsOut, dstIdx ); + + if (dstIdx < maxContactCapacity) + { + __global struct b3Contact4Data* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = -normalOnSurfaceB1; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB; + c->m_worldPosB[0] = pOnB1; + + c->m_childIndexA = -1; + c->m_childIndexB = faceIndex; + + GET_NPOINTS(*c) = 1; + } + } + + } + }//if (hasCollision) + +} + + + +// work-in-progress +__kernel void findConcaveSphereContactsKernel( __global int4* concavePairs, + __global const BodyData* rigidBodies, + __global const btCollidableGpu* collidables, + __global const ConvexPolyhedronCL* convexShapes, + __global const float4* vertices, + __global const float4* uniqueEdges, + __global const btGpuFace* faces, + __global const int* indices, + __global btAabbCL* aabbs, + __global struct b3Contact4Data* restrict globalContactsOut, + counter32_t nGlobalContactsOut, + int numConcavePairs, int maxContactCapacity + ) +{ + + int i = get_global_id(0); + if (i>=numConcavePairs) + return; + int pairIdx = i; + + int bodyIndexA = concavePairs[i].x; + int bodyIndexB = concavePairs[i].y; + + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + if (collidables[collidableIndexB].m_shapeType==SHAPE_SPHERE) + { + int f = concavePairs[i].z; + btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f]; + + float4 verticesA[3]; + for (int i=0;i<3;i++) + { + int index = indices[face.m_indexOffset+i]; + float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index]; + verticesA[i] = vert; + } + + float4 spherePos = rigidBodies[bodyIndexB].m_pos; + float sphereRadius = collidables[collidableIndexB].m_radius; + float4 convexPos = rigidBodies[bodyIndexA].m_pos; + float4 convexOrn = rigidBodies[bodyIndexA].m_quat; + + computeContactSphereTriangle(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, + rigidBodies,collidables, + verticesA, + globalContactsOut, nGlobalContactsOut,maxContactCapacity, + spherePos,sphereRadius,convexPos,convexOrn, f); + + return; + } +} \ No newline at end of file diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h new file mode 100644 index 000000000000..b0103fe67488 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h @@ -0,0 +1,1289 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* primitiveContactsKernelsCL= \ +"#ifndef B3_CONTACT4DATA_H\n" +"#define B3_CONTACT4DATA_H\n" +"#ifndef B3_FLOAT4_H\n" +"#define B3_FLOAT4_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#define B3_PLATFORM_DEFINITIONS_H\n" +"struct MyTest\n" +"{\n" +" int bla;\n" +"};\n" +"#ifdef __cplusplus\n" +"#else\n" +"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" +"#define B3_LARGE_FLOAT 1e18f\n" +"#define B3_INFINITY 1e18f\n" +"#define b3Assert(a)\n" +"#define b3ConstArray(a) __global const a*\n" +"#define b3AtomicInc atomic_inc\n" +"#define b3AtomicAdd atomic_add\n" +"#define b3Fabs fabs\n" +"#define b3Sqrt native_sqrt\n" +"#define b3Sin native_sin\n" +"#define b3Cos native_cos\n" +"#define B3_STATIC\n" +"#endif\n" +"#endif\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Float4;\n" +" #define b3Float4ConstArg const b3Float4\n" +" #define b3MakeFloat4 (float4)\n" +" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return dot(a1, b1);\n" +" }\n" +" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return cross(a1, b1);\n" +" }\n" +" #define b3MinFloat4 min\n" +" #define b3MaxFloat4 max\n" +" #define b3Normalized(a) normalize(a)\n" +"#endif \n" +" \n" +"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" +"{\n" +" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" +" return false;\n" +" return true;\n" +"}\n" +"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" +"{\n" +" float maxDot = -B3_INFINITY;\n" +" int i = 0;\n" +" int ptIndex = -1;\n" +" for( i = 0; i < vecLen; i++ )\n" +" {\n" +" float dot = b3Dot3F4(vecArray[i],vec);\n" +" \n" +" if( dot > maxDot )\n" +" {\n" +" maxDot = dot;\n" +" ptIndex = i;\n" +" }\n" +" }\n" +" b3Assert(ptIndex>=0);\n" +" if (ptIndex<0)\n" +" {\n" +" ptIndex = 0;\n" +" }\n" +" *dotOut = maxDot;\n" +" return ptIndex;\n" +"}\n" +"#endif //B3_FLOAT4_H\n" +"typedef struct b3Contact4Data b3Contact4Data_t;\n" +"struct b3Contact4Data\n" +"{\n" +" b3Float4 m_worldPosB[4];\n" +"// b3Float4 m_localPosA[4];\n" +"// b3Float4 m_localPosB[4];\n" +" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" +" unsigned short m_restituitionCoeffCmp;\n" +" unsigned short m_frictionCoeffCmp;\n" +" int m_batchIdx;\n" +" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" +" int m_bodyBPtrAndSignBit;\n" +" int m_childIndexA;\n" +" int m_childIndexB;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"};\n" +"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" +"{\n" +" return (int)contact->m_worldNormalOnB.w;\n" +"};\n" +"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" +"{\n" +" contact->m_worldNormalOnB.w = (float)numPoints;\n" +"};\n" +"#endif //B3_CONTACT4DATA_H\n" +"#define SHAPE_CONVEX_HULL 3\n" +"#define SHAPE_PLANE 4\n" +"#define SHAPE_CONCAVE_TRIMESH 5\n" +"#define SHAPE_COMPOUND_OF_CONVEX_HULLS 6\n" +"#define SHAPE_SPHERE 7\n" +"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" +"#ifdef cl_ext_atomic_counters_32\n" +"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" +"#else\n" +"#define counter32_t volatile __global int*\n" +"#endif\n" +"#define GET_GROUP_IDX get_group_id(0)\n" +"#define GET_LOCAL_IDX get_local_id(0)\n" +"#define GET_GLOBAL_IDX get_global_id(0)\n" +"#define GET_GROUP_SIZE get_local_size(0)\n" +"#define GET_NUM_GROUPS get_num_groups(0)\n" +"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" +"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" +"#define AtomInc(x) atom_inc(&(x))\n" +"#define AtomInc1(x, out) out = atom_inc(&(x))\n" +"#define AppendInc(x, out) out = atomic_inc(x)\n" +"#define AtomAdd(x, value) atom_add(&(x), value)\n" +"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" +"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" +"#define max2 max\n" +"#define min2 min\n" +"typedef unsigned int u32;\n" +"typedef struct \n" +"{\n" +" union\n" +" {\n" +" float4 m_min;\n" +" float m_minElems[4];\n" +" int m_minIndices[4];\n" +" };\n" +" union\n" +" {\n" +" float4 m_max;\n" +" float m_maxElems[4];\n" +" int m_maxIndices[4];\n" +" };\n" +"} btAabbCL;\n" +"///keep this in sync with btCollidable.h\n" +"typedef struct\n" +"{\n" +" int m_numChildShapes;\n" +" float m_radius;\n" +" int m_shapeType;\n" +" int m_shapeIndex;\n" +" \n" +"} btCollidableGpu;\n" +"typedef struct\n" +"{\n" +" float4 m_childPosition;\n" +" float4 m_childOrientation;\n" +" int m_shapeIndex;\n" +" int m_unused0;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"} btGpuChildShape;\n" +"#define GET_NPOINTS(x) (x).m_worldNormalOnB.w\n" +"typedef struct\n" +"{\n" +" float4 m_pos;\n" +" float4 m_quat;\n" +" float4 m_linVel;\n" +" float4 m_angVel;\n" +" u32 m_collidableIdx; \n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"} BodyData;\n" +"typedef struct \n" +"{\n" +" float4 m_localCenter;\n" +" float4 m_extents;\n" +" float4 mC;\n" +" float4 mE;\n" +" \n" +" float m_radius;\n" +" int m_faceOffset;\n" +" int m_numFaces;\n" +" int m_numVertices;\n" +" \n" +" int m_vertexOffset;\n" +" int m_uniqueEdgesOffset;\n" +" int m_numUniqueEdges;\n" +" int m_unused;\n" +"} ConvexPolyhedronCL;\n" +"typedef struct\n" +"{\n" +" float4 m_plane;\n" +" int m_indexOffset;\n" +" int m_numIndices;\n" +"} btGpuFace;\n" +"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" +"#define make_float4 (float4)\n" +"#define make_float2 (float2)\n" +"#define make_uint4 (uint4)\n" +"#define make_int4 (int4)\n" +"#define make_uint2 (uint2)\n" +"#define make_int2 (int2)\n" +"__inline\n" +"float fastDiv(float numerator, float denominator)\n" +"{\n" +" return native_divide(numerator, denominator); \n" +"// return numerator/denominator; \n" +"}\n" +"__inline\n" +"float4 fastDiv4(float4 numerator, float4 denominator)\n" +"{\n" +" return native_divide(numerator, denominator); \n" +"}\n" +"__inline\n" +"float4 cross3(float4 a, float4 b)\n" +"{\n" +" return cross(a,b);\n" +"}\n" +"//#define dot3F4 dot\n" +"__inline\n" +"float dot3F4(float4 a, float4 b)\n" +"{\n" +" float4 a1 = make_float4(a.xyz,0.f);\n" +" float4 b1 = make_float4(b.xyz,0.f);\n" +" return dot(a1, b1);\n" +"}\n" +"__inline\n" +"float4 fastNormalize4(float4 v)\n" +"{\n" +" return fast_normalize(v);\n" +"}\n" +"///////////////////////////////////////\n" +"// Quaternion\n" +"///////////////////////////////////////\n" +"typedef float4 Quaternion;\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b);\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in);\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec);\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q);\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b)\n" +"{\n" +" Quaternion ans;\n" +" ans = cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in)\n" +"{\n" +" return fastNormalize4(in);\n" +"// in /= length( in );\n" +"// return in;\n" +"}\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec)\n" +"{\n" +" Quaternion qInv = qtInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q)\n" +"{\n" +" return (Quaternion)(-q.xyz, q.w);\n" +"}\n" +"__inline\n" +"float4 qtInvRotate(const Quaternion q, float4 vec)\n" +"{\n" +" return qtRotate( qtInvert( q ), vec );\n" +"}\n" +"__inline\n" +"float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)\n" +"{\n" +" return qtRotate( *orientation, *p ) + (*translation);\n" +"}\n" +"void trInverse(float4 translationIn, Quaternion orientationIn,\n" +" float4* translationOut, Quaternion* orientationOut)\n" +"{\n" +" *orientationOut = qtInvert(orientationIn);\n" +" *translationOut = qtRotate(*orientationOut, -translationIn);\n" +"}\n" +"void trMul(float4 translationA, Quaternion orientationA,\n" +" float4 translationB, Quaternion orientationB,\n" +" float4* translationOut, Quaternion* orientationOut)\n" +"{\n" +" *orientationOut = qtMul(orientationA,orientationB);\n" +" *translationOut = transform(&translationB,&translationA,&orientationA);\n" +"}\n" +"__inline\n" +"float4 normalize3(const float4 a)\n" +"{\n" +" float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" +" return fastNormalize4( n );\n" +"}\n" +"__inline float4 lerp3(const float4 a,const float4 b, float t)\n" +"{\n" +" return make_float4( a.x + (b.x - a.x) * t,\n" +" a.y + (b.y - a.y) * t,\n" +" a.z + (b.z - a.z) * t,\n" +" 0.f);\n" +"}\n" +"float signedDistanceFromPointToPlane(float4 point, float4 planeEqn, float4* closestPointOnFace)\n" +"{\n" +" float4 n = (float4)(planeEqn.x, planeEqn.y, planeEqn.z, 0);\n" +" float dist = dot3F4(n, point) + planeEqn.w;\n" +" *closestPointOnFace = point - dist * n;\n" +" return dist;\n" +"}\n" +"inline bool IsPointInPolygon(float4 p, \n" +" const btGpuFace* face,\n" +" __global const float4* baseVertex,\n" +" __global const int* convexIndices,\n" +" float4* out)\n" +"{\n" +" float4 a;\n" +" float4 b;\n" +" float4 ab;\n" +" float4 ap;\n" +" float4 v;\n" +" float4 plane = make_float4(face->m_plane.x,face->m_plane.y,face->m_plane.z,0.f);\n" +" \n" +" if (face->m_numIndices<2)\n" +" return false;\n" +" \n" +" float4 v0 = baseVertex[convexIndices[face->m_indexOffset + face->m_numIndices-1]];\n" +" \n" +" b = v0;\n" +" for(unsigned i=0; i != face->m_numIndices; ++i)\n" +" {\n" +" a = b;\n" +" float4 vi = baseVertex[convexIndices[face->m_indexOffset + i]];\n" +" b = vi;\n" +" ab = b-a;\n" +" ap = p-a;\n" +" v = cross3(ab,plane);\n" +" if (dot(ap, v) > 0.f)\n" +" {\n" +" float ab_m2 = dot(ab, ab);\n" +" float rt = ab_m2 != 0.f ? dot(ab, ap) / ab_m2 : 0.f;\n" +" if (rt <= 0.f)\n" +" {\n" +" *out = a;\n" +" }\n" +" else if (rt >= 1.f) \n" +" {\n" +" *out = b;\n" +" }\n" +" else\n" +" {\n" +" float s = 1.f - rt;\n" +" out[0].x = s * a.x + rt * b.x;\n" +" out[0].y = s * a.y + rt * b.y;\n" +" out[0].z = s * a.z + rt * b.z;\n" +" }\n" +" return false;\n" +" }\n" +" }\n" +" return true;\n" +"}\n" +"void computeContactSphereConvex(int pairIndex,\n" +" int bodyIndexA, int bodyIndexB, \n" +" int collidableIndexA, int collidableIndexB, \n" +" __global const BodyData* rigidBodies, \n" +" __global const btCollidableGpu* collidables,\n" +" __global const ConvexPolyhedronCL* convexShapes,\n" +" __global const float4* convexVertices,\n" +" __global const int* convexIndices,\n" +" __global const btGpuFace* faces,\n" +" __global struct b3Contact4Data* restrict globalContactsOut,\n" +" counter32_t nGlobalContactsOut,\n" +" int maxContactCapacity,\n" +" float4 spherePos2,\n" +" float radius,\n" +" float4 pos,\n" +" float4 quat\n" +" )\n" +"{\n" +" float4 invPos;\n" +" float4 invOrn;\n" +" trInverse(pos,quat, &invPos,&invOrn);\n" +" float4 spherePos = transform(&spherePos2,&invPos,&invOrn);\n" +" int shapeIndex = collidables[collidableIndexB].m_shapeIndex;\n" +" int numFaces = convexShapes[shapeIndex].m_numFaces;\n" +" float4 closestPnt = (float4)(0, 0, 0, 0);\n" +" float4 hitNormalWorld = (float4)(0, 0, 0, 0);\n" +" float minDist = -1000000.f;\n" +" bool bCollide = true;\n" +" for ( int f = 0; f < numFaces; f++ )\n" +" {\n" +" btGpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];\n" +" // set up a plane equation \n" +" float4 planeEqn;\n" +" float4 n1 = face.m_plane;\n" +" n1.w = 0.f;\n" +" planeEqn = n1;\n" +" planeEqn.w = face.m_plane.w;\n" +" \n" +" \n" +" // compute a signed distance from the vertex in cloth to the face of rigidbody.\n" +" float4 pntReturn;\n" +" float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);\n" +" // If the distance is positive, the plane is a separating plane. \n" +" if ( dist > radius )\n" +" {\n" +" bCollide = false;\n" +" break;\n" +" }\n" +" if (dist>0)\n" +" {\n" +" //might hit an edge or vertex\n" +" float4 out;\n" +" float4 zeroPos = make_float4(0,0,0,0);\n" +" bool isInPoly = IsPointInPolygon(spherePos,\n" +" &face,\n" +" &convexVertices[convexShapes[shapeIndex].m_vertexOffset],\n" +" convexIndices,\n" +" &out);\n" +" if (isInPoly)\n" +" {\n" +" if (dist>minDist)\n" +" {\n" +" minDist = dist;\n" +" closestPnt = pntReturn;\n" +" hitNormalWorld = planeEqn;\n" +" \n" +" }\n" +" } else\n" +" {\n" +" float4 tmp = spherePos-out;\n" +" float l2 = dot(tmp,tmp);\n" +" if (l2minDist)\n" +" {\n" +" minDist = dist;\n" +" closestPnt = out;\n" +" hitNormalWorld = tmp/dist;\n" +" \n" +" }\n" +" \n" +" } else\n" +" {\n" +" bCollide = false;\n" +" break;\n" +" }\n" +" }\n" +" } else\n" +" {\n" +" if ( dist > minDist )\n" +" {\n" +" minDist = dist;\n" +" closestPnt = pntReturn;\n" +" hitNormalWorld.xyz = planeEqn.xyz;\n" +" }\n" +" }\n" +" \n" +" }\n" +" \n" +" if (bCollide && minDist > -10000)\n" +" {\n" +" float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);\n" +" float4 pOnB1 = transform(&closestPnt,&pos,&quat);\n" +" \n" +" float actualDepth = minDist-radius;\n" +" if (actualDepth<=0.f)\n" +" {\n" +" \n" +" pOnB1.w = actualDepth;\n" +" int dstIdx;\n" +" AppendInc( nGlobalContactsOut, dstIdx );\n" +" \n" +" \n" +" if (1)//dstIdx < maxContactCapacity)\n" +" {\n" +" __global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n" +" c->m_worldNormalOnB = -normalOnSurfaceB1;\n" +" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" +" c->m_batchIdx = pairIndex;\n" +" c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;\n" +" c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;\n" +" c->m_worldPosB[0] = pOnB1;\n" +" c->m_childIndexA = -1;\n" +" c->m_childIndexB = -1;\n" +" GET_NPOINTS(*c) = 1;\n" +" } \n" +" }\n" +" }//if (hasCollision)\n" +"}\n" +" \n" +"int extractManifoldSequential(const float4* p, int nPoints, float4 nearNormal, int4* contactIdx)\n" +"{\n" +" if( nPoints == 0 )\n" +" return 0;\n" +" \n" +" if (nPoints <=4)\n" +" return nPoints;\n" +" \n" +" \n" +" if (nPoints >64)\n" +" nPoints = 64;\n" +" \n" +" float4 center = make_float4(0.f);\n" +" {\n" +" \n" +" for (int i=0;im_numVertices;i++)\n" +" {\n" +" float4 vtx = convexVertices[hullB->m_vertexOffset+i];\n" +" float curDot = dot(vtx,planeNormalInConvex);\n" +" if (curDot>maxDot)\n" +" {\n" +" hitVertex=i;\n" +" maxDot=curDot;\n" +" hitVtx = vtx;\n" +" //make sure the deepest points is always included\n" +" if (numPoints==MAX_PLANE_CONVEX_POINTS)\n" +" numPoints--;\n" +" }\n" +" if (numPoints4)\n" +" {\n" +" numReducedPoints = extractManifoldSequential( contactPoints, numPoints, planeNormalInConvex, &contactIdx);\n" +" }\n" +" if (numReducedPoints>0)\n" +" {\n" +" int dstIdx;\n" +" AppendInc( nGlobalContactsOut, dstIdx );\n" +" if (dstIdx < maxContactCapacity)\n" +" {\n" +" resultIndex = dstIdx;\n" +" __global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n" +" c->m_worldNormalOnB = -planeNormalWorld;\n" +" //c->setFrictionCoeff(0.7);\n" +" //c->setRestituitionCoeff(0.f);\n" +" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" +" c->m_batchIdx = pairIndex;\n" +" c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;\n" +" c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;\n" +" c->m_childIndexA = -1;\n" +" c->m_childIndexB = -1;\n" +" switch (numReducedPoints)\n" +" {\n" +" case 4:\n" +" c->m_worldPosB[3] = contactPoints[contactIdx.w];\n" +" case 3:\n" +" c->m_worldPosB[2] = contactPoints[contactIdx.z];\n" +" case 2:\n" +" c->m_worldPosB[1] = contactPoints[contactIdx.y];\n" +" case 1:\n" +" c->m_worldPosB[0] = contactPoints[contactIdx.x];\n" +" default:\n" +" {\n" +" }\n" +" };\n" +" \n" +" GET_NPOINTS(*c) = numReducedPoints;\n" +" }//if (dstIdx < numPairs)\n" +" } \n" +" return resultIndex;\n" +"}\n" +"void computeContactPlaneSphere(int pairIndex,\n" +" int bodyIndexA, int bodyIndexB, \n" +" int collidableIndexA, int collidableIndexB, \n" +" __global const BodyData* rigidBodies, \n" +" __global const btCollidableGpu* collidables,\n" +" __global const btGpuFace* faces,\n" +" __global struct b3Contact4Data* restrict globalContactsOut,\n" +" counter32_t nGlobalContactsOut,\n" +" int maxContactCapacity)\n" +"{\n" +" float4 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane;\n" +" float radius = collidables[collidableIndexB].m_radius;\n" +" float4 posA1 = rigidBodies[bodyIndexA].m_pos;\n" +" float4 ornA1 = rigidBodies[bodyIndexA].m_quat;\n" +" float4 posB1 = rigidBodies[bodyIndexB].m_pos;\n" +" float4 ornB1 = rigidBodies[bodyIndexB].m_quat;\n" +" \n" +" bool hasCollision = false;\n" +" float4 planeNormal1 = make_float4(planeEq.x,planeEq.y,planeEq.z,0.f);\n" +" float planeConstant = planeEq.w;\n" +" float4 convexInPlaneTransPos1; Quaternion convexInPlaneTransOrn1;\n" +" {\n" +" float4 invPosA;Quaternion invOrnA;\n" +" trInverse(posA1,ornA1,&invPosA,&invOrnA);\n" +" trMul(invPosA,invOrnA,posB1,ornB1,&convexInPlaneTransPos1,&convexInPlaneTransOrn1);\n" +" }\n" +" float4 planeInConvexPos1; Quaternion planeInConvexOrn1;\n" +" {\n" +" float4 invPosB;Quaternion invOrnB;\n" +" trInverse(posB1,ornB1,&invPosB,&invOrnB);\n" +" trMul(invPosB,invOrnB,posA1,ornA1,&planeInConvexPos1,&planeInConvexOrn1); \n" +" }\n" +" float4 vtx1 = qtRotate(planeInConvexOrn1,-planeNormal1)*radius;\n" +" float4 vtxInPlane1 = transform(&vtx1,&convexInPlaneTransPos1,&convexInPlaneTransOrn1);\n" +" float distance = dot3F4(planeNormal1,vtxInPlane1) - planeConstant;\n" +" hasCollision = distance < 0.f;//m_manifoldPtr->getContactBreakingThreshold();\n" +" if (hasCollision)\n" +" {\n" +" float4 vtxInPlaneProjected1 = vtxInPlane1 - distance*planeNormal1;\n" +" float4 vtxInPlaneWorld1 = transform(&vtxInPlaneProjected1,&posA1,&ornA1);\n" +" float4 normalOnSurfaceB1 = qtRotate(ornA1,planeNormal1);\n" +" float4 pOnB1 = vtxInPlaneWorld1+normalOnSurfaceB1*distance;\n" +" pOnB1.w = distance;\n" +" int dstIdx;\n" +" AppendInc( nGlobalContactsOut, dstIdx );\n" +" \n" +" if (dstIdx < maxContactCapacity)\n" +" {\n" +" __global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n" +" c->m_worldNormalOnB = -normalOnSurfaceB1;\n" +" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" +" c->m_batchIdx = pairIndex;\n" +" c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;\n" +" c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;\n" +" c->m_worldPosB[0] = pOnB1;\n" +" c->m_childIndexA = -1;\n" +" c->m_childIndexB = -1;\n" +" GET_NPOINTS(*c) = 1;\n" +" }//if (dstIdx < numPairs)\n" +" }//if (hasCollision)\n" +"}\n" +"__kernel void primitiveContactsKernel( __global int4* pairs, \n" +" __global const BodyData* rigidBodies, \n" +" __global const btCollidableGpu* collidables,\n" +" __global const ConvexPolyhedronCL* convexShapes, \n" +" __global const float4* vertices,\n" +" __global const float4* uniqueEdges,\n" +" __global const btGpuFace* faces,\n" +" __global const int* indices,\n" +" __global struct b3Contact4Data* restrict globalContactsOut,\n" +" counter32_t nGlobalContactsOut,\n" +" int numPairs, int maxContactCapacity)\n" +"{\n" +" int i = get_global_id(0);\n" +" int pairIndex = i;\n" +" \n" +" float4 worldVertsB1[64];\n" +" float4 worldVertsB2[64];\n" +" int capacityWorldVerts = 64; \n" +" float4 localContactsOut[64];\n" +" int localContactCapacity=64;\n" +" \n" +" float minDist = -1e30f;\n" +" float maxDist = 0.02f;\n" +" if (i=0)\n" +" pairs[pairIndex].z = contactIndex;\n" +" return;\n" +" }\n" +" if (collidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&\n" +" collidables[collidableIndexB].m_shapeType == SHAPE_PLANE)\n" +" {\n" +" float4 posA;\n" +" posA = rigidBodies[bodyIndexA].m_pos;\n" +" Quaternion ornA;\n" +" ornA = rigidBodies[bodyIndexA].m_quat;\n" +" int contactIndex = computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, \n" +" rigidBodies,collidables,convexShapes,vertices,indices,\n" +" faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);\n" +" if (contactIndex>=0)\n" +" pairs[pairIndex].z = contactIndex;\n" +" return;\n" +" }\n" +" if (collidables[collidableIndexA].m_shapeType == SHAPE_PLANE &&\n" +" collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)\n" +" {\n" +" computeContactPlaneSphere(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n" +" rigidBodies,collidables,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);\n" +" return;\n" +" }\n" +" if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&\n" +" collidables[collidableIndexB].m_shapeType == SHAPE_PLANE)\n" +" {\n" +" computeContactPlaneSphere( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, \n" +" rigidBodies,collidables,\n" +" faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity);\n" +" return;\n" +" }\n" +" \n" +" \n" +" if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&\n" +" collidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)\n" +" {\n" +" \n" +" float4 spherePos = rigidBodies[bodyIndexA].m_pos;\n" +" float sphereRadius = collidables[collidableIndexA].m_radius;\n" +" float4 convexPos = rigidBodies[bodyIndexB].m_pos;\n" +" float4 convexOrn = rigidBodies[bodyIndexB].m_quat;\n" +" computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n" +" rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n" +" spherePos,sphereRadius,convexPos,convexOrn);\n" +" return;\n" +" }\n" +" if (collidables[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&\n" +" collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)\n" +" {\n" +" \n" +" float4 spherePos = rigidBodies[bodyIndexB].m_pos;\n" +" float sphereRadius = collidables[collidableIndexB].m_radius;\n" +" float4 convexPos = rigidBodies[bodyIndexA].m_pos;\n" +" float4 convexOrn = rigidBodies[bodyIndexA].m_quat;\n" +" computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, \n" +" rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n" +" spherePos,sphereRadius,convexPos,convexOrn);\n" +" return;\n" +" }\n" +" \n" +" \n" +" \n" +" \n" +" \n" +" \n" +" if (collidables[collidableIndexA].m_shapeType == SHAPE_SPHERE &&\n" +" collidables[collidableIndexB].m_shapeType == SHAPE_SPHERE)\n" +" {\n" +" //sphere-sphere\n" +" float radiusA = collidables[collidableIndexA].m_radius;\n" +" float radiusB = collidables[collidableIndexB].m_radius;\n" +" float4 posA = rigidBodies[bodyIndexA].m_pos;\n" +" float4 posB = rigidBodies[bodyIndexB].m_pos;\n" +" float4 diff = posA-posB;\n" +" float len = length(diff);\n" +" \n" +" ///iff distance positive, don't generate a new contact\n" +" if ( len <= (radiusA+radiusB))\n" +" {\n" +" ///distance (negative means penetration)\n" +" float dist = len - (radiusA+radiusB);\n" +" float4 normalOnSurfaceB = make_float4(1.f,0.f,0.f,0.f);\n" +" if (len > 0.00001)\n" +" {\n" +" normalOnSurfaceB = diff / len;\n" +" }\n" +" float4 contactPosB = posB + normalOnSurfaceB*radiusB;\n" +" contactPosB.w = dist;\n" +" \n" +" int dstIdx;\n" +" AppendInc( nGlobalContactsOut, dstIdx );\n" +" \n" +" if (dstIdx < maxContactCapacity)\n" +" {\n" +" __global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n" +" c->m_worldNormalOnB = normalOnSurfaceB;\n" +" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" +" c->m_batchIdx = pairIndex;\n" +" int bodyA = pairs[pairIndex].x;\n" +" int bodyB = pairs[pairIndex].y;\n" +" c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;\n" +" c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;\n" +" c->m_worldPosB[0] = contactPosB;\n" +" c->m_childIndexA = -1;\n" +" c->m_childIndexB = -1;\n" +" GET_NPOINTS(*c) = 1;\n" +" }//if (dstIdx < numPairs)\n" +" }//if ( len <= (radiusA+radiusB))\n" +" return;\n" +" }//SHAPE_SPHERE SHAPE_SPHERE\n" +" }// if (i= 0)\n" +" {\n" +" collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;\n" +" float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;\n" +" float4 childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;\n" +" float4 newPosA = qtRotate(ornA,childPosA)+posA;\n" +" float4 newOrnA = qtMul(ornA,childOrnA);\n" +" posA = newPosA;\n" +" ornA = newOrnA;\n" +" } else\n" +" {\n" +" collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" +" }\n" +" \n" +" if (childShapeIndexB>=0)\n" +" {\n" +" collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;\n" +" float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;\n" +" float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;\n" +" float4 newPosB = transform(&childPosB,&posB,&ornB);\n" +" float4 newOrnB = qtMul(ornB,childOrnB);\n" +" posB = newPosB;\n" +" ornB = newOrnB;\n" +" } else\n" +" {\n" +" collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; \n" +" }\n" +" \n" +" int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" +" int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" +" \n" +" int shapeTypeA = collidables[collidableIndexA].m_shapeType;\n" +" int shapeTypeB = collidables[collidableIndexB].m_shapeType;\n" +" int pairIndex = i;\n" +" if ((shapeTypeA == SHAPE_PLANE) && (shapeTypeB==SHAPE_CONVEX_HULL))\n" +" {\n" +" computeContactPlaneConvex( pairIndex, bodyIndexA,bodyIndexB, collidableIndexA,collidableIndexB, \n" +" rigidBodies,collidables,convexShapes,vertices,indices,\n" +" faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posB,ornB);\n" +" return;\n" +" }\n" +" if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB==SHAPE_PLANE))\n" +" {\n" +" computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, \n" +" rigidBodies,collidables,convexShapes,vertices,indices,\n" +" faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);\n" +" return;\n" +" }\n" +" if ((shapeTypeA == SHAPE_CONVEX_HULL) && (shapeTypeB == SHAPE_SPHERE))\n" +" {\n" +" float4 spherePos = rigidBodies[bodyIndexB].m_pos;\n" +" float sphereRadius = collidables[collidableIndexB].m_radius;\n" +" float4 convexPos = posA;\n" +" float4 convexOrn = ornA;\n" +" \n" +" computeContactSphereConvex(pairIndex, bodyIndexB, bodyIndexA , collidableIndexB,collidableIndexA, \n" +" rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n" +" spherePos,sphereRadius,convexPos,convexOrn);\n" +" \n" +" return;\n" +" }\n" +" if ((shapeTypeA == SHAPE_SPHERE) && (shapeTypeB == SHAPE_CONVEX_HULL))\n" +" {\n" +" float4 spherePos = rigidBodies[bodyIndexA].m_pos;\n" +" float sphereRadius = collidables[collidableIndexA].m_radius;\n" +" float4 convexPos = posB;\n" +" float4 convexOrn = ornB;\n" +" \n" +" computeContactSphereConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n" +" rigidBodies,collidables,convexShapes,vertices,indices,faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n" +" spherePos,sphereRadius,convexPos,convexOrn);\n" +" \n" +" return;\n" +" }\n" +" }// if (i 0 && r2 > 0 && r3 > 0 )\n" +" return true;\n" +" if ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) \n" +" return true;\n" +" return false;\n" +"}\n" +"float segmentSqrDistance(float4 from, float4 to,float4 p, float4* nearest) \n" +"{\n" +" float4 diff = p - from;\n" +" float4 v = to - from;\n" +" float t = dot(v,diff);\n" +" \n" +" if (t > 0) \n" +" {\n" +" float dotVV = dot(v,v);\n" +" if (t < dotVV) \n" +" {\n" +" t /= dotVV;\n" +" diff -= t*v;\n" +" } else \n" +" {\n" +" t = 1;\n" +" diff -= v;\n" +" }\n" +" } else\n" +" {\n" +" t = 0;\n" +" }\n" +" *nearest = from + t*v;\n" +" return dot(diff,diff); \n" +"}\n" +"void computeContactSphereTriangle(int pairIndex,\n" +" int bodyIndexA, int bodyIndexB,\n" +" int collidableIndexA, int collidableIndexB, \n" +" __global const BodyData* rigidBodies, \n" +" __global const btCollidableGpu* collidables,\n" +" const float4* triangleVertices,\n" +" __global struct b3Contact4Data* restrict globalContactsOut,\n" +" counter32_t nGlobalContactsOut,\n" +" int maxContactCapacity,\n" +" float4 spherePos2,\n" +" float radius,\n" +" float4 pos,\n" +" float4 quat,\n" +" int faceIndex\n" +" )\n" +"{\n" +" float4 invPos;\n" +" float4 invOrn;\n" +" trInverse(pos,quat, &invPos,&invOrn);\n" +" float4 spherePos = transform(&spherePos2,&invPos,&invOrn);\n" +" int numFaces = 3;\n" +" float4 closestPnt = (float4)(0, 0, 0, 0);\n" +" float4 hitNormalWorld = (float4)(0, 0, 0, 0);\n" +" float minDist = -1000000.f;\n" +" bool bCollide = false;\n" +" \n" +" //////////////////////////////////////\n" +" float4 sphereCenter;\n" +" sphereCenter = spherePos;\n" +" const float4* vertices = triangleVertices;\n" +" float contactBreakingThreshold = 0.f;//todo?\n" +" float radiusWithThreshold = radius + contactBreakingThreshold;\n" +" float4 edge10;\n" +" edge10 = vertices[1]-vertices[0];\n" +" edge10.w = 0.f;//is this needed?\n" +" float4 edge20;\n" +" edge20 = vertices[2]-vertices[0];\n" +" edge20.w = 0.f;//is this needed?\n" +" float4 normal = cross3(edge10,edge20);\n" +" normal = normalize(normal);\n" +" float4 p1ToCenter;\n" +" p1ToCenter = sphereCenter - vertices[0];\n" +" \n" +" float distanceFromPlane = dot(p1ToCenter,normal);\n" +" if (distanceFromPlane < 0.f)\n" +" {\n" +" //triangle facing the other way\n" +" distanceFromPlane *= -1.f;\n" +" normal *= -1.f;\n" +" }\n" +" hitNormalWorld = normal;\n" +" bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;\n" +" \n" +" // Check for contact / intersection\n" +" bool hasContact = false;\n" +" float4 contactPoint;\n" +" if (isInsideContactPlane) \n" +" {\n" +" \n" +" if (pointInTriangle(vertices,&normal, &sphereCenter)) \n" +" {\n" +" // Inside the contact wedge - touches a point on the shell plane\n" +" hasContact = true;\n" +" contactPoint = sphereCenter - normal*distanceFromPlane;\n" +" \n" +" } else {\n" +" // Could be inside one of the contact capsules\n" +" float contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;\n" +" float4 nearestOnEdge;\n" +" int numEdges = 3;\n" +" for (int i = 0; i < numEdges; i++) \n" +" {\n" +" float4 pa =vertices[i];\n" +" float4 pb = vertices[(i+1)%3];\n" +" float distanceSqr = segmentSqrDistance(pa,pb,sphereCenter, &nearestOnEdge);\n" +" if (distanceSqr < contactCapsuleRadiusSqr) \n" +" {\n" +" // Yep, we're inside a capsule\n" +" hasContact = true;\n" +" contactPoint = nearestOnEdge;\n" +" \n" +" }\n" +" \n" +" }\n" +" }\n" +" }\n" +" if (hasContact) \n" +" {\n" +" closestPnt = contactPoint;\n" +" float4 contactToCenter = sphereCenter - contactPoint;\n" +" minDist = length(contactToCenter);\n" +" if (minDist>FLT_EPSILON)\n" +" {\n" +" hitNormalWorld = normalize(contactToCenter);//*(1./minDist);\n" +" bCollide = true;\n" +" }\n" +" \n" +" }\n" +" /////////////////////////////////////\n" +" if (bCollide && minDist > -10000)\n" +" {\n" +" \n" +" float4 normalOnSurfaceB1 = qtRotate(quat,-hitNormalWorld);\n" +" float4 pOnB1 = transform(&closestPnt,&pos,&quat);\n" +" float actualDepth = minDist-radius;\n" +" \n" +" if (actualDepth<=0.f)\n" +" {\n" +" pOnB1.w = actualDepth;\n" +" int dstIdx;\n" +" \n" +" float lenSqr = dot3F4(normalOnSurfaceB1,normalOnSurfaceB1);\n" +" if (lenSqr>FLT_EPSILON)\n" +" {\n" +" AppendInc( nGlobalContactsOut, dstIdx );\n" +" \n" +" if (dstIdx < maxContactCapacity)\n" +" {\n" +" __global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n" +" c->m_worldNormalOnB = -normalOnSurfaceB1;\n" +" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" +" c->m_batchIdx = pairIndex;\n" +" c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;\n" +" c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;\n" +" c->m_worldPosB[0] = pOnB1;\n" +" c->m_childIndexA = -1;\n" +" c->m_childIndexB = faceIndex;\n" +" GET_NPOINTS(*c) = 1;\n" +" } \n" +" }\n" +" }\n" +" }//if (hasCollision)\n" +"}\n" +"// work-in-progress\n" +"__kernel void findConcaveSphereContactsKernel( __global int4* concavePairs,\n" +" __global const BodyData* rigidBodies,\n" +" __global const btCollidableGpu* collidables,\n" +" __global const ConvexPolyhedronCL* convexShapes, \n" +" __global const float4* vertices,\n" +" __global const float4* uniqueEdges,\n" +" __global const btGpuFace* faces,\n" +" __global const int* indices,\n" +" __global btAabbCL* aabbs,\n" +" __global struct b3Contact4Data* restrict globalContactsOut,\n" +" counter32_t nGlobalContactsOut,\n" +" int numConcavePairs, int maxContactCapacity\n" +" )\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numConcavePairs)\n" +" return;\n" +" int pairIdx = i;\n" +" int bodyIndexA = concavePairs[i].x;\n" +" int bodyIndexB = concavePairs[i].y;\n" +" int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" +" int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;\n" +" int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" +" int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" +" if (collidables[collidableIndexB].m_shapeType==SHAPE_SPHERE)\n" +" {\n" +" int f = concavePairs[i].z;\n" +" btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];\n" +" \n" +" float4 verticesA[3];\n" +" for (int i=0;i<3;i++)\n" +" {\n" +" int index = indices[face.m_indexOffset+i];\n" +" float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];\n" +" verticesA[i] = vert;\n" +" }\n" +" float4 spherePos = rigidBodies[bodyIndexB].m_pos;\n" +" float sphereRadius = collidables[collidableIndexB].m_radius;\n" +" float4 convexPos = rigidBodies[bodyIndexA].m_pos;\n" +" float4 convexOrn = rigidBodies[bodyIndexA].m_quat;\n" +" computeContactSphereTriangle(i, bodyIndexB, bodyIndexA, collidableIndexB, collidableIndexA, \n" +" rigidBodies,collidables,\n" +" verticesA,\n" +" globalContactsOut, nGlobalContactsOut,maxContactCapacity,\n" +" spherePos,sphereRadius,convexPos,convexOrn, f);\n" +" return;\n" +" }\n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl new file mode 100644 index 000000000000..a6565fd6fac9 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl @@ -0,0 +1,2018 @@ +//keep this enum in sync with the CPU version (in btCollidable.h) +//written by Erwin Coumans + + +#define SHAPE_CONVEX_HULL 3 +#define SHAPE_CONCAVE_TRIMESH 5 +#define TRIANGLE_NUM_CONVEX_FACES 5 +#define SHAPE_COMPOUND_OF_CONVEX_HULLS 6 + +#define B3_MAX_STACK_DEPTH 256 + + +typedef unsigned int u32; + +///keep this in sync with btCollidable.h +typedef struct +{ + union { + int m_numChildShapes; + int m_bvhIndex; + }; + union + { + float m_radius; + int m_compoundBvhIndex; + }; + + int m_shapeType; + int m_shapeIndex; + +} btCollidableGpu; + +#define MAX_NUM_PARTS_IN_BITS 10 + +///b3QuantizedBvhNode is a compressed aabb node, 16 bytes. +///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). +typedef struct +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes + int m_escapeIndexOrTriangleIndex; +} b3QuantizedBvhNode; + +typedef struct +{ + float4 m_aabbMin; + float4 m_aabbMax; + float4 m_quantization; + int m_numNodes; + int m_numSubTrees; + int m_nodeOffset; + int m_subTreeOffset; + +} b3BvhInfo; + + +int getTriangleIndex(const b3QuantizedBvhNode* rootNode) +{ + unsigned int x=0; + unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS); + // Get only the lower bits where the triangle index is stored + return (rootNode->m_escapeIndexOrTriangleIndex&~(y)); +} + +int getTriangleIndexGlobal(__global const b3QuantizedBvhNode* rootNode) +{ + unsigned int x=0; + unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS); + // Get only the lower bits where the triangle index is stored + return (rootNode->m_escapeIndexOrTriangleIndex&~(y)); +} + +int isLeafNode(const b3QuantizedBvhNode* rootNode) +{ + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0; +} + +int isLeafNodeGlobal(__global const b3QuantizedBvhNode* rootNode) +{ + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0; +} + +int getEscapeIndex(const b3QuantizedBvhNode* rootNode) +{ + return -rootNode->m_escapeIndexOrTriangleIndex; +} + +int getEscapeIndexGlobal(__global const b3QuantizedBvhNode* rootNode) +{ + return -rootNode->m_escapeIndexOrTriangleIndex; +} + + +typedef struct +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes, points to the root of the subtree + int m_rootNodeIndex; + //4 bytes + int m_subtreeSize; + int m_padding[3]; +} b3BvhSubtreeInfo; + + + + + + + +typedef struct +{ + float4 m_childPosition; + float4 m_childOrientation; + int m_shapeIndex; + int m_unused0; + int m_unused1; + int m_unused2; +} btGpuChildShape; + + +typedef struct +{ + float4 m_pos; + float4 m_quat; + float4 m_linVel; + float4 m_angVel; + + u32 m_collidableIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} BodyData; + + +typedef struct +{ + float4 m_localCenter; + float4 m_extents; + float4 mC; + float4 mE; + + float m_radius; + int m_faceOffset; + int m_numFaces; + int m_numVertices; + + int m_vertexOffset; + int m_uniqueEdgesOffset; + int m_numUniqueEdges; + int m_unused; +} ConvexPolyhedronCL; + +typedef struct +{ + union + { + float4 m_min; + float m_minElems[4]; + int m_minIndices[4]; + }; + union + { + float4 m_max; + float m_maxElems[4]; + int m_maxIndices[4]; + }; +} btAabbCL; + +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Common/shared/b3Int2.h" + + + +typedef struct +{ + float4 m_plane; + int m_indexOffset; + int m_numIndices; +} btGpuFace; + +#define make_float4 (float4) + + +__inline +float4 cross3(float4 a, float4 b) +{ + return cross(a,b); + + +// float4 a1 = make_float4(a.xyz,0.f); +// float4 b1 = make_float4(b.xyz,0.f); + +// return cross(a1,b1); + +//float4 c = make_float4(a.y*b.z - a.z*b.y,a.z*b.x - a.x*b.z,a.x*b.y - a.y*b.x,0.f); + + // float4 c = make_float4(a.y*b.z - a.z*b.y,1.f,a.x*b.y - a.y*b.x,0.f); + + //return c; +} + +__inline +float dot3F4(float4 a, float4 b) +{ + float4 a1 = make_float4(a.xyz,0.f); + float4 b1 = make_float4(b.xyz,0.f); + return dot(a1, b1); +} + +__inline +float4 fastNormalize4(float4 v) +{ + v = make_float4(v.xyz,0.f); + return fast_normalize(v); +} + + +/////////////////////////////////////// +// Quaternion +/////////////////////////////////////// + +typedef float4 Quaternion; + +__inline +Quaternion qtMul(Quaternion a, Quaternion b); + +__inline +Quaternion qtNormalize(Quaternion in); + +__inline +float4 qtRotate(Quaternion q, float4 vec); + +__inline +Quaternion qtInvert(Quaternion q); + + + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b) +{ + Quaternion ans; + ans = cross3( a, b ); + ans += a.w*b+b.w*a; +// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - dot3F4(a, b); + return ans; +} + +__inline +Quaternion qtNormalize(Quaternion in) +{ + return fastNormalize4(in); +// in /= length( in ); +// return in; +} +__inline +float4 qtRotate(Quaternion q, float4 vec) +{ + Quaternion qInv = qtInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = qtMul(qtMul(q,vcpy),qInv); + return out; +} + +__inline +Quaternion qtInvert(Quaternion q) +{ + return (Quaternion)(-q.xyz, q.w); +} + +__inline +float4 qtInvRotate(const Quaternion q, float4 vec) +{ + return qtRotate( qtInvert( q ), vec ); +} + +__inline +float4 transform(const float4* p, const float4* translation, const Quaternion* orientation) +{ + return qtRotate( *orientation, *p ) + (*translation); +} + + + +__inline +float4 normalize3(const float4 a) +{ + float4 n = make_float4(a.x, a.y, a.z, 0.f); + return fastNormalize4( n ); +} + +inline void projectLocal(const ConvexPolyhedronCL* hull, const float4 pos, const float4 orn, +const float4* dir, const float4* vertices, float* min, float* max) +{ + min[0] = FLT_MAX; + max[0] = -FLT_MAX; + int numVerts = hull->m_numVertices; + + const float4 localDir = qtInvRotate(orn,*dir); + float offset = dot(pos,*dir); + for(int i=0;im_vertexOffset+i],localDir); + if(dp < min[0]) + min[0] = dp; + if(dp > max[0]) + max[0] = dp; + } + if(min[0]>max[0]) + { + float tmp = min[0]; + min[0] = max[0]; + max[0] = tmp; + } + min[0] += offset; + max[0] += offset; +} + +inline void project(__global const ConvexPolyhedronCL* hull, const float4 pos, const float4 orn, +const float4* dir, __global const float4* vertices, float* min, float* max) +{ + min[0] = FLT_MAX; + max[0] = -FLT_MAX; + int numVerts = hull->m_numVertices; + + const float4 localDir = qtInvRotate(orn,*dir); + float offset = dot(pos,*dir); + for(int i=0;im_vertexOffset+i],localDir); + if(dp < min[0]) + min[0] = dp; + if(dp > max[0]) + max[0] = dp; + } + if(min[0]>max[0]) + { + float tmp = min[0]; + min[0] = max[0]; + max[0] = tmp; + } + min[0] += offset; + max[0] += offset; +} + +inline bool TestSepAxisLocalA(const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, + const float4 posA,const float4 ornA, + const float4 posB,const float4 ornB, + float4* sep_axis, const float4* verticesA, __global const float4* verticesB,float* depth) +{ + float Min0,Max0; + float Min1,Max1; + projectLocal(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0); + project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1); + + if(Max01e-6f || fabs(v.y)>1e-6f || fabs(v.z)>1e-6f) + return false; + return true; +} + + + +bool findSeparatingAxisLocalA( const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, + const float4 posA1, + const float4 ornA, + const float4 posB1, + const float4 ornB, + const float4 DeltaC2, + + const float4* verticesA, + const float4* uniqueEdgesA, + const btGpuFace* facesA, + const int* indicesA, + + __global const float4* verticesB, + __global const float4* uniqueEdgesB, + __global const btGpuFace* facesB, + __global const int* indicesB, + float4* sep, + float* dmin) +{ + + + float4 posA = posA1; + posA.w = 0.f; + float4 posB = posB1; + posB.w = 0.f; + int curPlaneTests=0; + { + int numFacesA = hullA->m_numFaces; + // Test normals from hullA + for(int i=0;im_faceOffset+i].m_plane; + float4 faceANormalWS = qtRotate(ornA,normal); + if (dot3F4(DeltaC2,faceANormalWS)<0) + faceANormalWS*=-1.f; + curPlaneTests++; + float d; + if(!TestSepAxisLocalA( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d)) + return false; + if(d<*dmin) + { + *dmin = d; + *sep = faceANormalWS; + } + } + } + if((dot3F4(-DeltaC2,*sep))>0.0f) + { + *sep = -(*sep); + } + return true; +} + +bool findSeparatingAxisLocalB( __global const ConvexPolyhedronCL* hullA, const ConvexPolyhedronCL* hullB, + const float4 posA1, + const float4 ornA, + const float4 posB1, + const float4 ornB, + const float4 DeltaC2, + __global const float4* verticesA, + __global const float4* uniqueEdgesA, + __global const btGpuFace* facesA, + __global const int* indicesA, + const float4* verticesB, + const float4* uniqueEdgesB, + const btGpuFace* facesB, + const int* indicesB, + float4* sep, + float* dmin) +{ + + + float4 posA = posA1; + posA.w = 0.f; + float4 posB = posB1; + posB.w = 0.f; + int curPlaneTests=0; + { + int numFacesA = hullA->m_numFaces; + // Test normals from hullA + for(int i=0;im_faceOffset+i].m_plane; + float4 faceANormalWS = qtRotate(ornA,normal); + if (dot3F4(DeltaC2,faceANormalWS)<0) + faceANormalWS *= -1.f; + curPlaneTests++; + float d; + if(!TestSepAxisLocalA( hullB, hullA, posB,ornB,posA,ornA, &faceANormalWS, verticesB,verticesA, &d)) + return false; + if(d<*dmin) + { + *dmin = d; + *sep = faceANormalWS; + } + } + } + if((dot3F4(-DeltaC2,*sep))>0.0f) + { + *sep = -(*sep); + } + return true; +} + + + +bool findSeparatingAxisEdgeEdgeLocalA( const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, + const float4 posA1, + const float4 ornA, + const float4 posB1, + const float4 ornB, + const float4 DeltaC2, + const float4* verticesA, + const float4* uniqueEdgesA, + const btGpuFace* facesA, + const int* indicesA, + __global const float4* verticesB, + __global const float4* uniqueEdgesB, + __global const btGpuFace* facesB, + __global const int* indicesB, + float4* sep, + float* dmin) +{ + + + float4 posA = posA1; + posA.w = 0.f; + float4 posB = posB1; + posB.w = 0.f; + + int curPlaneTests=0; + + int curEdgeEdge = 0; + // Test edges + for(int e0=0;e0m_numUniqueEdges;e0++) + { + const float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0]; + float4 edge0World = qtRotate(ornA,edge0); + + for(int e1=0;e1m_numUniqueEdges;e1++) + { + const float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1]; + float4 edge1World = qtRotate(ornB,edge1); + + + float4 crossje = cross3(edge0World,edge1World); + + curEdgeEdge++; + if(!IsAlmostZero(crossje)) + { + crossje = normalize3(crossje); + if (dot3F4(DeltaC2,crossje)<0) + crossje *= -1.f; + + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + projectLocal(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); + project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); + + if(Max00.0f) + { + *sep = -(*sep); + } + return true; +} + + +inline bool TestSepAxis(__global const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, + const float4 posA,const float4 ornA, + const float4 posB,const float4 ornB, + float4* sep_axis, __global const float4* vertices,float* depth) +{ + float Min0,Max0; + float Min1,Max1; + project(hullA,posA,ornA,sep_axis,vertices, &Min0, &Max0); + project(hullB,posB,ornB, sep_axis,vertices, &Min1, &Max1); + + if(Max0m_numFaces; + // Test normals from hullA + for(int i=0;im_faceOffset+i].m_plane; + float4 faceANormalWS = qtRotate(ornA,normal); + + if (dot3F4(DeltaC2,faceANormalWS)<0) + faceANormalWS*=-1.f; + + curPlaneTests++; + + float d; + if(!TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, vertices,&d)) + return false; + + if(d<*dmin) + { + *dmin = d; + *sep = faceANormalWS; + } + } + } + + + if((dot3F4(-DeltaC2,*sep))>0.0f) + { + *sep = -(*sep); + } + + return true; +} + + + + +bool findSeparatingAxisUnitSphere( __global const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, + const float4 posA1, + const float4 ornA, + const float4 posB1, + const float4 ornB, + const float4 DeltaC2, + __global const float4* vertices, + __global const float4* unitSphereDirections, + int numUnitSphereDirections, + float4* sep, + float* dmin) +{ + + float4 posA = posA1; + posA.w = 0.f; + float4 posB = posB1; + posB.w = 0.f; + + int curPlaneTests=0; + + int curEdgeEdge = 0; + // Test unit sphere directions + for (int i=0;i0) + crossje *= -1.f; + { + float dist; + bool result = true; + float Min0,Max0; + float Min1,Max1; + project(hullA,posA,ornA,&crossje,vertices, &Min0, &Max0); + project(hullB,posB,ornB,&crossje,vertices, &Min1, &Max1); + + if(Max00.0f) + { + *sep = -(*sep); + } + return true; +} + + +bool findSeparatingAxisEdgeEdge( __global const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, + const float4 posA1, + const float4 ornA, + const float4 posB1, + const float4 ornB, + const float4 DeltaC2, + __global const float4* vertices, + __global const float4* uniqueEdges, + __global const btGpuFace* faces, + __global const int* indices, + float4* sep, + float* dmin) +{ + + + float4 posA = posA1; + posA.w = 0.f; + float4 posB = posB1; + posB.w = 0.f; + + int curPlaneTests=0; + + int curEdgeEdge = 0; + // Test edges + for(int e0=0;e0m_numUniqueEdges;e0++) + { + const float4 edge0 = uniqueEdges[hullA->m_uniqueEdgesOffset+e0]; + float4 edge0World = qtRotate(ornA,edge0); + + for(int e1=0;e1m_numUniqueEdges;e1++) + { + const float4 edge1 = uniqueEdges[hullB->m_uniqueEdgesOffset+e1]; + float4 edge1World = qtRotate(ornB,edge1); + + + float4 crossje = cross3(edge0World,edge1World); + + curEdgeEdge++; + if(!IsAlmostZero(crossje)) + { + crossje = normalize3(crossje); + if (dot3F4(DeltaC2,crossje)<0) + crossje*=-1.f; + + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + project(hullA,posA,ornA,&crossje,vertices, &Min0, &Max0); + project(hullB,posB,ornB,&crossje,vertices, &Min1, &Max1); + + if(Max00.0f) + { + *sep = -(*sep); + } + return true; +} + + +// work-in-progress +__kernel void processCompoundPairsKernel( __global const int4* gpuCompoundPairs, + __global const BodyData* rigidBodies, + __global const btCollidableGpu* collidables, + __global const ConvexPolyhedronCL* convexShapes, + __global const float4* vertices, + __global const float4* uniqueEdges, + __global const btGpuFace* faces, + __global const int* indices, + __global btAabbCL* aabbs, + __global const btGpuChildShape* gpuChildShapes, + __global volatile float4* gpuCompoundSepNormalsOut, + __global volatile int* gpuHasCompoundSepNormalsOut, + int numCompoundPairs + ) +{ + + int i = get_global_id(0); + if (i= 0) + { + collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex; + float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition; + float4 childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation; + float4 newPosA = qtRotate(ornA,childPosA)+posA; + float4 newOrnA = qtMul(ornA,childOrnA); + posA = newPosA; + ornA = newOrnA; + } else + { + collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + } + + if (childShapeIndexB>=0) + { + collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex; + float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition; + float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation; + float4 newPosB = transform(&childPosB,&posB,&ornB); + float4 newOrnB = qtMul(ornB,childOrnB); + posB = newPosB; + ornB = newOrnB; + } else + { + collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + } + + gpuHasCompoundSepNormalsOut[i] = 0; + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + int shapeTypeA = collidables[collidableIndexA].m_shapeType; + int shapeTypeB = collidables[collidableIndexB].m_shapeType; + + + if ((shapeTypeA != SHAPE_CONVEX_HULL) || (shapeTypeB != SHAPE_CONVEX_HULL)) + { + return; + } + + int hasSeparatingAxis = 5; + + int numFacesA = convexShapes[shapeIndexA].m_numFaces; + float dmin = FLT_MAX; + posA.w = 0.f; + posB.w = 0.f; + float4 c0local = convexShapes[shapeIndexA].m_localCenter; + float4 c0 = transform(&c0local, &posA, &ornA); + float4 c1local = convexShapes[shapeIndexB].m_localCenter; + float4 c1 = transform(&c1local,&posB,&ornB); + const float4 DeltaC2 = c0 - c1; + float4 sepNormal = make_float4(1,0,0,0); + bool sepA = findSeparatingAxis( &convexShapes[shapeIndexA], &convexShapes[shapeIndexB],posA,ornA,posB,ornB,DeltaC2,vertices,uniqueEdges,faces,indices,&sepNormal,&dmin); + hasSeparatingAxis = 4; + if (!sepA) + { + hasSeparatingAxis = 0; + } else + { + bool sepB = findSeparatingAxis( &convexShapes[shapeIndexB],&convexShapes[shapeIndexA],posB,ornB,posA,ornA,DeltaC2,vertices,uniqueEdges,faces,indices,&sepNormal,&dmin); + + if (!sepB) + { + hasSeparatingAxis = 0; + } else//(!sepB) + { + bool sepEE = findSeparatingAxisEdgeEdge( &convexShapes[shapeIndexA], &convexShapes[shapeIndexB],posA,ornA,posB,ornB,DeltaC2,vertices,uniqueEdges,faces,indices,&sepNormal,&dmin); + if (sepEE) + { + gpuCompoundSepNormalsOut[i] = sepNormal;//fastNormalize4(sepNormal); + gpuHasCompoundSepNormalsOut[i] = 1; + }//sepEE + }//(!sepB) + }//(!sepA) + + + } + +} + + +inline b3Float4 MyUnQuantize(const unsigned short* vecIn, b3Float4 quantization, b3Float4 bvhAabbMin) +{ + b3Float4 vecOut; + vecOut = b3MakeFloat4( + (float)(vecIn[0]) / (quantization.x), + (float)(vecIn[1]) / (quantization.y), + (float)(vecIn[2]) / (quantization.z), + 0.f); + + vecOut += bvhAabbMin; + return vecOut; +} + +inline b3Float4 MyUnQuantizeGlobal(__global const unsigned short* vecIn, b3Float4 quantization, b3Float4 bvhAabbMin) +{ + b3Float4 vecOut; + vecOut = b3MakeFloat4( + (float)(vecIn[0]) / (quantization.x), + (float)(vecIn[1]) / (quantization.y), + (float)(vecIn[2]) / (quantization.z), + 0.f); + + vecOut += bvhAabbMin; + return vecOut; +} + + +// work-in-progress +__kernel void findCompoundPairsKernel( __global const int4* pairs, + __global const BodyData* rigidBodies, + __global const btCollidableGpu* collidables, + __global const ConvexPolyhedronCL* convexShapes, + __global const float4* vertices, + __global const float4* uniqueEdges, + __global const btGpuFace* faces, + __global const int* indices, + __global b3Aabb_t* aabbLocalSpace, + __global const btGpuChildShape* gpuChildShapes, + __global volatile int4* gpuCompoundPairsOut, + __global volatile int* numCompoundPairsOut, + __global const b3BvhSubtreeInfo* subtrees, + __global const b3QuantizedBvhNode* quantizedNodes, + __global const b3BvhInfo* bvhInfos, + int numPairs, + int maxNumCompoundPairsCapacity + ) +{ + + int i = get_global_id(0); + + if (imaxStackDepth && !(isLeafA && isLeafB)) + { + //printf("Error: traversal exceeded maxStackDepth"); + continue; + } + + if(isInternalA) + { + int nodeAleftChild = node.x+1; + bool isNodeALeftChildLeaf = isLeafNodeGlobal(&quantizedNodes[node.x+1]); + int nodeArightChild = isNodeALeftChildLeaf? node.x+2 : node.x+1 + getEscapeIndexGlobal(&quantizedNodes[node.x+1]); + + if(isInternalB) + { + int nodeBleftChild = node.y+1; + bool isNodeBLeftChildLeaf = isLeafNodeGlobal(&quantizedNodes[node.y+1]); + int nodeBrightChild = isNodeBLeftChildLeaf? node.y+2 : node.y+1 + getEscapeIndexGlobal(&quantizedNodes[node.y+1]); + + nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBleftChild); + nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBleftChild); + nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBrightChild); + nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBrightChild); + } + else + { + nodeStack[depth++] = b3MakeInt2(nodeAleftChild,node.y); + nodeStack[depth++] = b3MakeInt2(nodeArightChild,node.y); + } + } + else + { + if(isInternalB) + { + int nodeBleftChild = node.y+1; + bool isNodeBLeftChildLeaf = isLeafNodeGlobal(&quantizedNodes[node.y+1]); + int nodeBrightChild = isNodeBLeftChildLeaf? node.y+2 : node.y+1 + getEscapeIndexGlobal(&quantizedNodes[node.y+1]); + nodeStack[depth++] = b3MakeInt2(node.x,nodeBleftChild); + nodeStack[depth++] = b3MakeInt2(node.x,nodeBrightChild); + } + else + { + int compoundPairIdx = atomic_inc(numCompoundPairsOut); + if (compoundPairIdxm_numFaces;face++) + { + const float4 Normal = make_float4(facesB[hullB->m_faceOffset+face].m_plane.x, + facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f); + const float4 WorldNormal = qtRotate(ornB, Normal); + float d = dot3F4(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + + { + const btGpuFace polyB = facesB[hullB->m_faceOffset+closestFaceB]; + int numVertices = polyB.m_numIndices; + if (numVertices>capacityWorldVerts) + numVertices = capacityWorldVerts; + + for(int e0=0;e0m_vertexOffset+indicesB[polyB.m_indexOffset+e0]]; + worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = transform(&b,&posB,&ornB); + } + } + } + + int closestFaceA=0; + { + float dmin = FLT_MAX; + for(int face=0;facem_numFaces;face++) + { + const float4 Normal = make_float4( + facesA[hullA->m_faceOffset+face].m_plane.x, + facesA[hullA->m_faceOffset+face].m_plane.y, + facesA[hullA->m_faceOffset+face].m_plane.z, + 0.f); + const float4 faceANormalWS = qtRotate(ornA,Normal); + + float d = dot3F4(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + worldNormalsA1[pairIndex] = faceANormalWS; + } + } + } + + int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices; + if (numVerticesA>capacityWorldVerts) + numVerticesA = capacityWorldVerts; + + for(int e0=0;e0m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]]; + worldVertsA1[pairIndex*capacityWorldVerts+e0] = transform(&a, &posA,&ornA); + } + } + + clippingFaces[pairIndex].x = closestFaceA; + clippingFaces[pairIndex].y = closestFaceB; + clippingFaces[pairIndex].z = numVerticesA; + clippingFaces[pairIndex].w = numWorldVertsB1; + + + return numContactsOut; +} + + + + +// work-in-progress +__kernel void findConcaveSeparatingAxisKernel( __global int4* concavePairs, + __global const BodyData* rigidBodies, + __global const btCollidableGpu* collidables, + __global const ConvexPolyhedronCL* convexShapes, + __global const float4* vertices, + __global const float4* uniqueEdges, + __global const btGpuFace* faces, + __global const int* indices, + __global const btGpuChildShape* gpuChildShapes, + __global btAabbCL* aabbs, + __global float4* concaveSeparatingNormalsOut, + __global int* concaveHasSeparatingNormals, + __global int4* clippingFacesOut, + __global float4* worldVertsA1GPU, + __global float4* worldNormalsAGPU, + __global float4* worldVertsB1GPU, + int vertexFaceCapacity, + int numConcavePairs + ) +{ + + int i = get_global_id(0); + if (i>=numConcavePairs) + return; + + concaveHasSeparatingNormals[i] = 0; + + int pairIdx = i; + + int bodyIndexA = concavePairs[i].x; + int bodyIndexB = concavePairs[i].y; + + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&& + collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + concavePairs[pairIdx].w = -1; + return; + } + + + + int numFacesA = convexShapes[shapeIndexA].m_numFaces; + int numActualConcaveConvexTests = 0; + + int f = concavePairs[i].z; + + bool overlap = false; + + ConvexPolyhedronCL convexPolyhedronA; + + //add 3 vertices of the triangle + convexPolyhedronA.m_numVertices = 3; + convexPolyhedronA.m_vertexOffset = 0; + float4 localCenter = make_float4(0.f,0.f,0.f,0.f); + + btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f]; + float4 triMinAabb, triMaxAabb; + btAabbCL triAabb; + triAabb.m_min = make_float4(1e30f,1e30f,1e30f,0.f); + triAabb.m_max = make_float4(-1e30f,-1e30f,-1e30f,0.f); + + float4 verticesA[3]; + for (int i=0;i<3;i++) + { + int index = indices[face.m_indexOffset+i]; + float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index]; + verticesA[i] = vert; + localCenter += vert; + + triAabb.m_min = min(triAabb.m_min,vert); + triAabb.m_max = max(triAabb.m_max,vert); + + } + + overlap = true; + overlap = (triAabb.m_min.x > aabbs[bodyIndexB].m_max.x || triAabb.m_max.x < aabbs[bodyIndexB].m_min.x) ? false : overlap; + overlap = (triAabb.m_min.z > aabbs[bodyIndexB].m_max.z || triAabb.m_max.z < aabbs[bodyIndexB].m_min.z) ? false : overlap; + overlap = (triAabb.m_min.y > aabbs[bodyIndexB].m_max.y || triAabb.m_max.y < aabbs[bodyIndexB].m_min.y) ? false : overlap; + + if (overlap) + { + float dmin = FLT_MAX; + int hasSeparatingAxis=5; + float4 sepAxis=make_float4(1,2,3,4); + + int localCC=0; + numActualConcaveConvexTests++; + + //a triangle has 3 unique edges + convexPolyhedronA.m_numUniqueEdges = 3; + convexPolyhedronA.m_uniqueEdgesOffset = 0; + float4 uniqueEdgesA[3]; + + uniqueEdgesA[0] = (verticesA[1]-verticesA[0]); + uniqueEdgesA[1] = (verticesA[2]-verticesA[1]); + uniqueEdgesA[2] = (verticesA[0]-verticesA[2]); + + + convexPolyhedronA.m_faceOffset = 0; + + float4 normal = make_float4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); + + btGpuFace facesA[TRIANGLE_NUM_CONVEX_FACES]; + int indicesA[3+3+2+2+2]; + int curUsedIndices=0; + int fidx=0; + + //front size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[0] = 0; + indicesA[1] = 1; + indicesA[2] = 2; + curUsedIndices+=3; + float c = face.m_plane.w; + facesA[fidx].m_plane.x = normal.x; + facesA[fidx].m_plane.y = normal.y; + facesA[fidx].m_plane.z = normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + //back size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[3]=2; + indicesA[4]=1; + indicesA[5]=0; + curUsedIndices+=3; + float c = dot(normal,verticesA[0]); + float c1 = -face.m_plane.w; + facesA[fidx].m_plane.x = -normal.x; + facesA[fidx].m_plane.y = -normal.y; + facesA[fidx].m_plane.z = -normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + + bool addEdgePlanes = true; + if (addEdgePlanes) + { + int numVertices=3; + int prevVertex = numVertices-1; + for (int i=0;i= 0, so output intersection + ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + } + } + else + { + if (de<0) + { + // Start >= 0, end < 0 so output intersection and end + ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + ppVtxOut[numVertsOut++] = endVertex; + } + } + firstVertex = endVertex; + ds = de; + } + return numVertsOut; +} + + + +// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut +int clipFace(const float4* pVtxIn, int numVertsIn, float4 planeNormalWS,float planeEqWS, float4* ppVtxOut) +{ + + int ve; + float ds, de; + int numVertsOut = 0; +//double-check next test + if (numVertsIn < 2) + return 0; + + float4 firstVertex=pVtxIn[numVertsIn-1]; + float4 endVertex = pVtxIn[0]; + + ds = dot3F4(planeNormalWS,firstVertex)+planeEqWS; + + for (ve = 0; ve < numVertsIn; ve++) + { + endVertex=pVtxIn[ve]; + + de = dot3F4(planeNormalWS,endVertex)+planeEqWS; + + if (ds<0) + { + if (de<0) + { + // Start < 0, end < 0, so output endVertex + ppVtxOut[numVertsOut++] = endVertex; + } + else + { + // Start < 0, end >= 0, so output intersection + ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + } + } + else + { + if (de<0) + { + // Start >= 0, end < 0 so output intersection and end + ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + ppVtxOut[numVertsOut++] = endVertex; + } + } + firstVertex = endVertex; + ds = de; + } + return numVertsOut; +} + + +int clipFaceAgainstHull(const float4 separatingNormal, __global const b3ConvexPolyhedronData_t* hullA, + const float4 posA, const Quaternion ornA, float4* worldVertsB1, int numWorldVertsB1, + float4* worldVertsB2, int capacityWorldVertsB2, + const float minDist, float maxDist, + __global const float4* vertices, + __global const b3GpuFace_t* faces, + __global const int* indices, + float4* contactsOut, + int contactCapacity) +{ + int numContactsOut = 0; + + float4* pVtxIn = worldVertsB1; + float4* pVtxOut = worldVertsB2; + + int numVertsIn = numWorldVertsB1; + int numVertsOut = 0; + + int closestFaceA=-1; + { + float dmin = FLT_MAX; + for(int face=0;facem_numFaces;face++) + { + const float4 Normal = make_float4( + faces[hullA->m_faceOffset+face].m_plane.x, + faces[hullA->m_faceOffset+face].m_plane.y, + faces[hullA->m_faceOffset+face].m_plane.z,0.f); + const float4 faceANormalWS = qtRotate(ornA,Normal); + + float d = dot3F4(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + } + } + } + if (closestFaceA<0) + return numContactsOut; + + b3GpuFace_t polyA = faces[hullA->m_faceOffset+closestFaceA]; + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + int numVerticesA = polyA.m_numIndices; + for(int e0=0;e0m_vertexOffset+indices[polyA.m_indexOffset+e0]]; + const float4 b = vertices[hullA->m_vertexOffset+indices[polyA.m_indexOffset+((e0+1)%numVerticesA)]]; + const float4 edge0 = a - b; + const float4 WorldEdge0 = qtRotate(ornA,edge0); + float4 planeNormalA = make_float4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + float4 worldPlaneAnormal1 = qtRotate(ornA,planeNormalA); + + float4 planeNormalWS1 = -cross3(WorldEdge0,worldPlaneAnormal1); + float4 worldA1 = transform(&a,&posA,&ornA); + float planeEqWS1 = -dot3F4(worldA1,planeNormalWS1); + + float4 planeNormalWS = planeNormalWS1; + float planeEqWS=planeEqWS1; + + //clip face + //clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS); + numVertsOut = clipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut); + + //btSwap(pVtxIn,pVtxOut); + float4* tmp = pVtxOut; + pVtxOut = pVtxIn; + pVtxIn = tmp; + numVertsIn = numVertsOut; + numVertsOut = 0; + } + + + // only keep points that are behind the witness face + { + float4 localPlaneNormal = make_float4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + float localPlaneEq = polyA.m_plane.w; + float4 planeNormalWS = qtRotate(ornA,localPlaneNormal); + float planeEqWS=localPlaneEq-dot3F4(planeNormalWS,posA); + for (int i=0;im_numFaces;face++) + { + const float4 Normal = make_float4( + facesA[hullA->m_faceOffset+face].m_plane.x, + facesA[hullA->m_faceOffset+face].m_plane.y, + facesA[hullA->m_faceOffset+face].m_plane.z,0.f); + const float4 faceANormalWS = qtRotate(ornA,Normal); + + float d = dot3F4(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + } + } + } + if (closestFaceA<0) + return numContactsOut; + + b3GpuFace_t polyA = facesA[hullA->m_faceOffset+closestFaceA]; + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + int numVerticesA = polyA.m_numIndices; + for(int e0=0;e0m_vertexOffset+indicesA[polyA.m_indexOffset+e0]]; + const float4 b = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+((e0+1)%numVerticesA)]]; + const float4 edge0 = a - b; + const float4 WorldEdge0 = qtRotate(ornA,edge0); + float4 planeNormalA = make_float4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + float4 worldPlaneAnormal1 = qtRotate(ornA,planeNormalA); + + float4 planeNormalWS1 = -cross3(WorldEdge0,worldPlaneAnormal1); + float4 worldA1 = transform(&a,&posA,&ornA); + float planeEqWS1 = -dot3F4(worldA1,planeNormalWS1); + + float4 planeNormalWS = planeNormalWS1; + float planeEqWS=planeEqWS1; + + //clip face + //clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS); + numVertsOut = clipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut); + + //btSwap(pVtxIn,pVtxOut); + float4* tmp = pVtxOut; + pVtxOut = pVtxIn; + pVtxIn = tmp; + numVertsIn = numVertsOut; + numVertsOut = 0; + } + + + // only keep points that are behind the witness face + { + float4 localPlaneNormal = make_float4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + float localPlaneEq = polyA.m_plane.w; + float4 planeNormalWS = qtRotate(ornA,localPlaneNormal); + float planeEqWS=localPlaneEq-dot3F4(planeNormalWS,posA); + for (int i=0;im_numFaces;face++) + { + const float4 Normal = make_float4(faces[hullB->m_faceOffset+face].m_plane.x, + faces[hullB->m_faceOffset+face].m_plane.y, faces[hullB->m_faceOffset+face].m_plane.z,0.f); + const float4 WorldNormal = qtRotate(ornB, Normal); + float d = dot3F4(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + + { + const b3GpuFace_t polyB = faces[hullB->m_faceOffset+closestFaceB]; + const int numVertices = polyB.m_numIndices; + for(int e0=0;e0m_vertexOffset+indices[polyB.m_indexOffset+e0]]; + worldVertsB1[numWorldVertsB1++] = transform(&b,&posB,&ornB); + } + } + + if (closestFaceB>=0) + { + numContactsOut = clipFaceAgainstHull(separatingNormal, hullA, + posA,ornA, + worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist,vertices, + faces, + indices,localContactsOut,localContactCapacity); + } + + return numContactsOut; +} + + +int clipHullAgainstHullLocalA(const float4 separatingNormal, + const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB, + const float4 posA, const Quaternion ornA,const float4 posB, const Quaternion ornB, + float4* worldVertsB1, float4* worldVertsB2, int capacityWorldVerts, + const float minDist, float maxDist, + const float4* verticesA, + const b3GpuFace_t* facesA, + const int* indicesA, + __global const float4* verticesB, + __global const b3GpuFace_t* facesB, + __global const int* indicesB, + float4* localContactsOut, + int localContactCapacity) +{ + int numContactsOut = 0; + int numWorldVertsB1= 0; + + + int closestFaceB=-1; + float dmax = -FLT_MAX; + + { + for(int face=0;facem_numFaces;face++) + { + const float4 Normal = make_float4(facesB[hullB->m_faceOffset+face].m_plane.x, + facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f); + const float4 WorldNormal = qtRotate(ornB, Normal); + float d = dot3F4(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + + { + const b3GpuFace_t polyB = facesB[hullB->m_faceOffset+closestFaceB]; + const int numVertices = polyB.m_numIndices; + for(int e0=0;e0m_vertexOffset+indicesB[polyB.m_indexOffset+e0]]; + worldVertsB1[numWorldVertsB1++] = transform(&b,&posB,&ornB); + } + } + + if (closestFaceB>=0) + { + numContactsOut = clipFaceAgainstHullLocalA(separatingNormal, hullA, + posA,ornA, + worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist, + verticesA,facesA,indicesA, + verticesB,facesB,indicesB, + localContactsOut,localContactCapacity); + } + + return numContactsOut; +} + +#define PARALLEL_SUM(v, n) for(int j=1; j v[i+offset].y)? v[i]: v[i+offset]; } +#define REDUCE_MIN(v, n) {int i=0;\ +for(int offset=0; offset64) + nPoints = 64; + + float4 center = make_float4(0.f); + { + + for (int i=0;i a[ie].x )? a[0].x: a[ie].x; + a[0].y = (a[0].y > a[ie].y )? a[0].y: a[ie].y; + a[0].z = (a[0].z > a[ie].z )? a[0].z: a[ie].z; + a[0].w = (a[0].w > a[ie].w )? a[0].w: a[ie].w; + } + + idx[0] = (int)a[0].x & 0xff; + idx[1] = (int)a[0].y & 0xff; + idx[2] = (int)a[0].z & 0xff; + idx[3] = (int)a[0].w & 0xff; + } + } + + { + float2 h[64]; + PARALLEL_DO( h[ie] = make_float2((float)ie, p[ie].w), nPoints ); + REDUCE_MIN( h, nPoints ); + max00 = h[0]; + } + } + + contactIdx[0] = idx[0]; + contactIdx[1] = idx[1]; + contactIdx[2] = idx[2]; + contactIdx[3] = idx[3]; + + + return 4; + } +} + + + +__kernel void extractManifoldAndAddContactKernel(__global const int4* pairs, + __global const b3RigidBodyData_t* rigidBodies, + __global const float4* closestPointsWorld, + __global const float4* separatingNormalsWorld, + __global const int* contactCounts, + __global const int* contactOffsets, + __global struct b3Contact4Data* restrict contactsOut, + counter32_t nContactsOut, + int contactCapacity, + int numPairs, + int pairIndex + ) +{ + int idx = get_global_id(0); + + if (idxm_worldNormalOnB = -normal; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = idx; + int bodyA = pairs[pairIndex].x; + int bodyB = pairs[pairIndex].y; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0 ? -bodyA:bodyA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0 ? -bodyB:bodyB; + c->m_childIndexA = -1; + c->m_childIndexB = -1; + for (int i=0;im_worldPosB[i] = localPoints[contactIdx[i]]; + } + GET_NPOINTS(*c) = nContacts; + } + } +} + + +void trInverse(float4 translationIn, Quaternion orientationIn, + float4* translationOut, Quaternion* orientationOut) +{ + *orientationOut = qtInvert(orientationIn); + *translationOut = qtRotate(*orientationOut, -translationIn); +} + +void trMul(float4 translationA, Quaternion orientationA, + float4 translationB, Quaternion orientationB, + float4* translationOut, Quaternion* orientationOut) +{ + *orientationOut = qtMul(orientationA,orientationB); + *translationOut = transform(&translationB,&translationA,&orientationA); +} + + + + +__kernel void clipHullHullKernel( __global int4* pairs, + __global const b3RigidBodyData_t* rigidBodies, + __global const b3Collidable_t* collidables, + __global const b3ConvexPolyhedronData_t* convexShapes, + __global const float4* vertices, + __global const float4* uniqueEdges, + __global const b3GpuFace_t* faces, + __global const int* indices, + __global const float4* separatingNormals, + __global const int* hasSeparatingAxis, + __global struct b3Contact4Data* restrict globalContactsOut, + counter32_t nGlobalContactsOut, + int numPairs, + int contactCapacity) +{ + + int i = get_global_id(0); + int pairIndex = i; + + float4 worldVertsB1[64]; + float4 worldVertsB2[64]; + int capacityWorldVerts = 64; + + float4 localContactsOut[64]; + int localContactCapacity=64; + + float minDist = -1e30f; + float maxDist = 0.02f; + + if (i0) + { + float4 normal = -separatingNormals[i]; + int nPoints = numLocalContactsOut; + float4* pointsIn = localContactsOut; + int contactIdx[4];// = {-1,-1,-1,-1}; + + contactIdx[0] = -1; + contactIdx[1] = -1; + contactIdx[2] = -1; + contactIdx[3] = -1; + + int nReducedContacts = extractManifoldSequential(pointsIn, nPoints, normal, contactIdx); + + + int mprContactIndex = pairs[pairIndex].z; + + int dstIdx = mprContactIndex; + if (dstIdx<0) + { + AppendInc( nGlobalContactsOut, dstIdx ); + } + + if (dstIdxm_worldNormalOnB = -normal; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + int bodyA = pairs[pairIndex].x; + int bodyB = pairs[pairIndex].y; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB; + c->m_childIndexA = -1; + c->m_childIndexB = -1; + + for (int i=0;i0||(mprContactIndex<0)) + { + c->m_worldPosB[i] = pointsIn[contactIdx[i]]; + } + } + GET_NPOINTS(*c) = nReducedContacts; + } + + }// if (numContactsOut>0) + }// if (hasSeparatingAxis[i]) + }// if (i= 0) + { + collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex; + float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition; + float4 childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation; + float4 newPosA = qtRotate(ornA,childPosA)+posA; + float4 newOrnA = qtMul(ornA,childOrnA); + posA = newPosA; + ornA = newOrnA; + } else + { + collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + } + + if (childShapeIndexB>=0) + { + collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex; + float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition; + float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation; + float4 newPosB = transform(&childPosB,&posB,&ornB); + float4 newOrnB = qtMul(ornB,childOrnB); + posB = newPosB; + ornB = newOrnB; + } else + { + collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + } + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + int numLocalContactsOut = clipHullAgainstHull(gpuCompoundSepNormalsOut[i], + &convexShapes[shapeIndexA], &convexShapes[shapeIndexB], + posA,ornA, + posB,ornB, + worldVertsB1,worldVertsB2,capacityWorldVerts, + minDist, maxDist, + vertices,faces,indices, + localContactsOut,localContactCapacity); + + if (numLocalContactsOut>0) + { + float4 normal = -gpuCompoundSepNormalsOut[i]; + int nPoints = numLocalContactsOut; + float4* pointsIn = localContactsOut; + int contactIdx[4];// = {-1,-1,-1,-1}; + + contactIdx[0] = -1; + contactIdx[1] = -1; + contactIdx[2] = -1; + contactIdx[3] = -1; + + int nReducedContacts = extractManifoldSequential(pointsIn, nPoints, normal, contactIdx); + + int dstIdx; + AppendInc( nGlobalContactsOut, dstIdx ); + if ((dstIdx+nReducedContacts) < maxContactCapacity) + { + __global struct b3Contact4Data* c = globalContactsOut+ dstIdx; + c->m_worldNormalOnB = -normal; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + int bodyA = gpuCompoundPairs[pairIndex].x; + int bodyB = gpuCompoundPairs[pairIndex].y; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB; + c->m_childIndexA = childShapeIndexA; + c->m_childIndexB = childShapeIndexB; + for (int i=0;im_worldPosB[i] = pointsIn[contactIdx[i]]; + } + GET_NPOINTS(*c) = nReducedContacts; + } + + }// if (numContactsOut>0) + }// if (gpuHasCompoundSepNormalsOut[i]) + }// if (i 0.00001) + { + normalOnSurfaceB = diff / len; + } + float4 contactPosB = posB + normalOnSurfaceB*radiusB; + contactPosB.w = dist; + + int dstIdx; + AppendInc( nGlobalContactsOut, dstIdx ); + if (dstIdx < contactCapacity) + { + __global struct b3Contact4Data* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = -normalOnSurfaceB; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + int bodyA = pairs[pairIndex].x; + int bodyB = pairs[pairIndex].y; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB; + c->m_worldPosB[0] = contactPosB; + c->m_childIndexA = -1; + c->m_childIndexB = -1; + + GET_NPOINTS(*c) = 1; + }//if (dstIdx < numPairs) + }//if ( len <= (radiusA+radiusB)) + }//SHAPE_SPHERE SHAPE_SPHERE + }//if (i0) + { + float4 normal = -separatingNormals[i]; + int nPoints = numLocalContactsOut; + float4* pointsIn = localContactsOut; + int contactIdx[4];// = {-1,-1,-1,-1}; + + contactIdx[0] = -1; + contactIdx[1] = -1; + contactIdx[2] = -1; + contactIdx[3] = -1; + + int nReducedContacts = extractManifoldSequential(pointsIn, nPoints, normal, contactIdx); + + int dstIdx; + AppendInc( nGlobalContactsOut, dstIdx ); + if (dstIdxm_worldNormalOnB = -normal; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + int bodyA = concavePairsIn[pairIndex].x; + int bodyB = concavePairsIn[pairIndex].y; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB; + c->m_childIndexA = childShapeIndexA; + c->m_childIndexB = childShapeIndexB; + for (int i=0;im_worldPosB[i] = pointsIn[contactIdx[i]]; + } + GET_NPOINTS(*c) = nReducedContacts; + } + + }// if (numContactsOut>0) + }// if (im_numFaces;face++) + { + const float4 Normal = make_float4(faces[hullB->m_faceOffset+face].m_plane.x, + faces[hullB->m_faceOffset+face].m_plane.y, faces[hullB->m_faceOffset+face].m_plane.z,0.f); + const float4 WorldNormal = qtRotate(ornB, Normal); + float d = dot3F4(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + + { + const b3GpuFace_t polyB = faces[hullB->m_faceOffset+closestFaceB]; + const int numVertices = polyB.m_numIndices; + for(int e0=0;e0m_vertexOffset+indices[polyB.m_indexOffset+e0]]; + worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = transform(&b,&posB,&ornB); + } + } + + int closestFaceA=-1; + { + float dmin = FLT_MAX; + for(int face=0;facem_numFaces;face++) + { + const float4 Normal = make_float4( + faces[hullA->m_faceOffset+face].m_plane.x, + faces[hullA->m_faceOffset+face].m_plane.y, + faces[hullA->m_faceOffset+face].m_plane.z, + 0.f); + const float4 faceANormalWS = qtRotate(ornA,Normal); + + float d = dot3F4(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + worldNormalsA1[pairIndex] = faceANormalWS; + } + } + } + + int numVerticesA = faces[hullA->m_faceOffset+closestFaceA].m_numIndices; + for(int e0=0;e0m_vertexOffset+indices[faces[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]]; + worldVertsA1[pairIndex*capacityWorldVerts+e0] = transform(&a, &posA,&ornA); + } + + clippingFaces[pairIndex].x = closestFaceA; + clippingFaces[pairIndex].y = closestFaceB; + clippingFaces[pairIndex].z = numVerticesA; + clippingFaces[pairIndex].w = numWorldVertsB1; + + + return numContactsOut; +} + + + +int clipFaces(__global float4* worldVertsA1, + __global float4* worldNormalsA1, + __global float4* worldVertsB1, + __global float4* worldVertsB2, + int capacityWorldVertsB2, + const float minDist, float maxDist, + __global int4* clippingFaces, + int pairIndex) +{ + int numContactsOut = 0; + + int closestFaceA = clippingFaces[pairIndex].x; + int closestFaceB = clippingFaces[pairIndex].y; + int numVertsInA = clippingFaces[pairIndex].z; + int numVertsInB = clippingFaces[pairIndex].w; + + int numVertsOut = 0; + + if (closestFaceA<0) + return numContactsOut; + + __global float4* pVtxIn = &worldVertsB1[pairIndex*capacityWorldVertsB2]; + __global float4* pVtxOut = &worldVertsB2[pairIndex*capacityWorldVertsB2]; + + + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + + for(int e0=0;e0=0) + { + + + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + + for(int e0=0;e00) + { + + __global float4* pointsIn = &worldVertsB2[pairIndex*vertexFaceCapacity]; + float4 normal = -separatingNormals[i]; + + int nReducedContacts = extractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx); + + int mprContactIndex = pairs[pairIndex].z; + + int dstIdx = mprContactIndex; + + if (dstIdx<0) + { + AppendInc( nGlobalContactsOut, dstIdx ); + } +//#if 0 + + if (dstIdx < contactCapacity) + { + + __global struct b3Contact4Data* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = -normal; + c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff); + c->m_batchIdx = pairIndex; + int bodyA = pairs[pairIndex].x; + int bodyB = pairs[pairIndex].y; + + pairs[pairIndex].w = dstIdx; + + c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB; + c->m_childIndexA =-1; + c->m_childIndexB =-1; + + switch (nReducedContacts) + { + case 4: + c->m_worldPosB[3] = pointsIn[contactIdx.w]; + case 3: + c->m_worldPosB[2] = pointsIn[contactIdx.z]; + case 2: + c->m_worldPosB[1] = pointsIn[contactIdx.y]; + case 1: + if (mprContactIndex<0)//test + c->m_worldPosB[0] = pointsIn[contactIdx.x]; + default: + { + } + }; + + GET_NPOINTS(*c) = nReducedContacts; + + } + + +//#endif + + }// if (numContactsOut>0) + }// if (hasSeparatingAxis[i]) + }// if (i1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" +" return false;\n" +" return true;\n" +"}\n" +"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" +"{\n" +" float maxDot = -B3_INFINITY;\n" +" int i = 0;\n" +" int ptIndex = -1;\n" +" for( i = 0; i < vecLen; i++ )\n" +" {\n" +" float dot = b3Dot3F4(vecArray[i],vec);\n" +" \n" +" if( dot > maxDot )\n" +" {\n" +" maxDot = dot;\n" +" ptIndex = i;\n" +" }\n" +" }\n" +" b3Assert(ptIndex>=0);\n" +" if (ptIndex<0)\n" +" {\n" +" ptIndex = 0;\n" +" }\n" +" *dotOut = maxDot;\n" +" return ptIndex;\n" +"}\n" +"#endif //B3_FLOAT4_H\n" +"typedef struct b3Contact4Data b3Contact4Data_t;\n" +"struct b3Contact4Data\n" +"{\n" +" b3Float4 m_worldPosB[4];\n" +"// b3Float4 m_localPosA[4];\n" +"// b3Float4 m_localPosB[4];\n" +" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" +" unsigned short m_restituitionCoeffCmp;\n" +" unsigned short m_frictionCoeffCmp;\n" +" int m_batchIdx;\n" +" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" +" int m_bodyBPtrAndSignBit;\n" +" int m_childIndexA;\n" +" int m_childIndexB;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"};\n" +"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" +"{\n" +" return (int)contact->m_worldNormalOnB.w;\n" +"};\n" +"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" +"{\n" +" contact->m_worldNormalOnB.w = (float)numPoints;\n" +"};\n" +"#endif //B3_CONTACT4DATA_H\n" +"#ifndef B3_CONVEX_POLYHEDRON_DATA_H\n" +"#define B3_CONVEX_POLYHEDRON_DATA_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_QUAT_H\n" +"#define B3_QUAT_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif\n" +"#endif\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Quat;\n" +" #define b3QuatConstArg const b3Quat\n" +" \n" +" \n" +"inline float4 b3FastNormalize4(float4 v)\n" +"{\n" +" v = (float4)(v.xyz,0.f);\n" +" return fast_normalize(v);\n" +"}\n" +" \n" +"inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" +"inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" +"inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" +"{\n" +" b3Quat ans;\n" +" ans = b3Cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - b3Dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" +"{\n" +" b3Quat q;\n" +" q=in;\n" +" //return b3FastNormalize4(in);\n" +" float len = native_sqrt(dot(q, q));\n" +" if(len > 0.f)\n" +" {\n" +" q *= 1.f / len;\n" +" }\n" +" else\n" +" {\n" +" q.x = q.y = q.z = 0.f;\n" +" q.w = 1.f;\n" +" }\n" +" return q;\n" +"}\n" +"inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" b3Quat qInv = b3QuatInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" return b3QuatRotate( b3QuatInvert( q ), vec );\n" +"}\n" +"inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" +"{\n" +" return b3QuatRotate( orientation, point ) + (translation);\n" +"}\n" +" \n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"typedef struct b3GpuFace b3GpuFace_t;\n" +"struct b3GpuFace\n" +"{\n" +" b3Float4 m_plane;\n" +" int m_indexOffset;\n" +" int m_numIndices;\n" +" int m_unusedPadding1;\n" +" int m_unusedPadding2;\n" +"};\n" +"typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;\n" +"struct b3ConvexPolyhedronData\n" +"{\n" +" b3Float4 m_localCenter;\n" +" b3Float4 m_extents;\n" +" b3Float4 mC;\n" +" b3Float4 mE;\n" +" float m_radius;\n" +" int m_faceOffset;\n" +" int m_numFaces;\n" +" int m_numVertices;\n" +" int m_vertexOffset;\n" +" int m_uniqueEdgesOffset;\n" +" int m_numUniqueEdges;\n" +" int m_unused;\n" +"};\n" +"#endif //B3_CONVEX_POLYHEDRON_DATA_H\n" +"#ifndef B3_COLLIDABLE_H\n" +"#define B3_COLLIDABLE_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"enum b3ShapeTypes\n" +"{\n" +" SHAPE_HEIGHT_FIELD=1,\n" +" SHAPE_CONVEX_HULL=3,\n" +" SHAPE_PLANE=4,\n" +" SHAPE_CONCAVE_TRIMESH=5,\n" +" SHAPE_COMPOUND_OF_CONVEX_HULLS=6,\n" +" SHAPE_SPHERE=7,\n" +" MAX_NUM_SHAPE_TYPES,\n" +"};\n" +"typedef struct b3Collidable b3Collidable_t;\n" +"struct b3Collidable\n" +"{\n" +" union {\n" +" int m_numChildShapes;\n" +" int m_bvhIndex;\n" +" };\n" +" union\n" +" {\n" +" float m_radius;\n" +" int m_compoundBvhIndex;\n" +" };\n" +" int m_shapeType;\n" +" int m_shapeIndex;\n" +"};\n" +"typedef struct b3GpuChildShape b3GpuChildShape_t;\n" +"struct b3GpuChildShape\n" +"{\n" +" b3Float4 m_childPosition;\n" +" b3Quat m_childOrientation;\n" +" int m_shapeIndex;\n" +" int m_unused0;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"};\n" +"struct b3CompoundOverlappingPair\n" +"{\n" +" int m_bodyIndexA;\n" +" int m_bodyIndexB;\n" +"// int m_pairType;\n" +" int m_childShapeIndexA;\n" +" int m_childShapeIndexB;\n" +"};\n" +"#endif //B3_COLLIDABLE_H\n" +"#ifndef B3_RIGIDBODY_DATA_H\n" +"#define B3_RIGIDBODY_DATA_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifndef B3_MAT3x3_H\n" +"#define B3_MAT3x3_H\n" +"#ifndef B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"typedef struct\n" +"{\n" +" b3Float4 m_row[3];\n" +"}b3Mat3x3;\n" +"#define b3Mat3x3ConstArg const b3Mat3x3\n" +"#define b3GetRow(m,row) (m.m_row[row])\n" +"inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" +"{\n" +" b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" +" b3Mat3x3 out;\n" +" out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" +" out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" +" out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" +" out.m_row[0].w = 0.f;\n" +" out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" +" out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" +" out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" +" out.m_row[1].w = 0.f;\n" +" out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" +" out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" +" out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" +" out.m_row[2].w = 0.f;\n" +" return out;\n" +"}\n" +"inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = fabs(matIn.m_row[0]);\n" +" out.m_row[1] = fabs(matIn.m_row[1]);\n" +" out.m_row[2] = fabs(matIn.m_row[2]);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtZero();\n" +"__inline\n" +"b3Mat3x3 mtIdentity();\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Mat3x3 mtZero()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(0.f);\n" +" m.m_row[1] = (b3Float4)(0.f);\n" +" m.m_row[2] = (b3Float4)(0.f);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtIdentity()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(1,0,0,0);\n" +" m.m_row[1] = (b3Float4)(0,1,0,0);\n" +" m.m_row[2] = (b3Float4)(0,0,1,0);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" +" out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" +" out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" +"{\n" +" b3Mat3x3 transB;\n" +" transB = mtTranspose( b );\n" +" b3Mat3x3 ans;\n" +" // why this doesn't run when 0ing in the for{}\n" +" a.m_row[0].w = 0.f;\n" +" a.m_row[1].w = 0.f;\n" +" a.m_row[2].w = 0.f;\n" +" for(int i=0; i<3; i++)\n" +" {\n" +"// a.m_row[i].w = 0.f;\n" +" ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" +" ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" +" ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" +" ans.m_row[i].w = 0.f;\n" +" }\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" +"{\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a.m_row[0], b );\n" +" ans.y = b3Dot3F4( a.m_row[1], b );\n" +" ans.z = b3Dot3F4( a.m_row[2], b );\n" +" ans.w = 0.f;\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" +"{\n" +" b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" +" b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" +" b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a, colx );\n" +" ans.y = b3Dot3F4( a, coly );\n" +" ans.z = b3Dot3F4( a, colz );\n" +" return ans;\n" +"}\n" +"#endif\n" +"#endif //B3_MAT3x3_H\n" +"typedef struct b3RigidBodyData b3RigidBodyData_t;\n" +"struct b3RigidBodyData\n" +"{\n" +" b3Float4 m_pos;\n" +" b3Quat m_quat;\n" +" b3Float4 m_linVel;\n" +" b3Float4 m_angVel;\n" +" int m_collidableIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"};\n" +"typedef struct b3InertiaData b3InertiaData_t;\n" +"struct b3InertiaData\n" +"{\n" +" b3Mat3x3 m_invInertiaWorld;\n" +" b3Mat3x3 m_initInvInertia;\n" +"};\n" +"#endif //B3_RIGIDBODY_DATA_H\n" +" \n" +"#define GET_NPOINTS(x) (x).m_worldNormalOnB.w\n" +"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" +"#define make_float4 (float4)\n" +"#define make_float2 (float2)\n" +"#define make_uint4 (uint4)\n" +"#define make_int4 (int4)\n" +"#define make_uint2 (uint2)\n" +"#define make_int2 (int2)\n" +"__inline\n" +"float fastDiv(float numerator, float denominator)\n" +"{\n" +" return native_divide(numerator, denominator); \n" +"// return numerator/denominator; \n" +"}\n" +"__inline\n" +"float4 fastDiv4(float4 numerator, float4 denominator)\n" +"{\n" +" return native_divide(numerator, denominator); \n" +"}\n" +"__inline\n" +"float4 cross3(float4 a, float4 b)\n" +"{\n" +" return cross(a,b);\n" +"}\n" +"//#define dot3F4 dot\n" +"__inline\n" +"float dot3F4(float4 a, float4 b)\n" +"{\n" +" float4 a1 = make_float4(a.xyz,0.f);\n" +" float4 b1 = make_float4(b.xyz,0.f);\n" +" return dot(a1, b1);\n" +"}\n" +"__inline\n" +"float4 fastNormalize4(float4 v)\n" +"{\n" +" return fast_normalize(v);\n" +"}\n" +"///////////////////////////////////////\n" +"// Quaternion\n" +"///////////////////////////////////////\n" +"typedef float4 Quaternion;\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b);\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in);\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec);\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q);\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b)\n" +"{\n" +" Quaternion ans;\n" +" ans = cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in)\n" +"{\n" +" return fastNormalize4(in);\n" +"// in /= length( in );\n" +"// return in;\n" +"}\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec)\n" +"{\n" +" Quaternion qInv = qtInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q)\n" +"{\n" +" return (Quaternion)(-q.xyz, q.w);\n" +"}\n" +"__inline\n" +"float4 qtInvRotate(const Quaternion q, float4 vec)\n" +"{\n" +" return qtRotate( qtInvert( q ), vec );\n" +"}\n" +"__inline\n" +"float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)\n" +"{\n" +" return qtRotate( *orientation, *p ) + (*translation);\n" +"}\n" +"__inline\n" +"float4 normalize3(const float4 a)\n" +"{\n" +" float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" +" return fastNormalize4( n );\n" +"}\n" +"__inline float4 lerp3(const float4 a,const float4 b, float t)\n" +"{\n" +" return make_float4( a.x + (b.x - a.x) * t,\n" +" a.y + (b.y - a.y) * t,\n" +" a.z + (b.z - a.z) * t,\n" +" 0.f);\n" +"}\n" +"// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut\n" +"int clipFaceGlobal(__global const float4* pVtxIn, int numVertsIn, float4 planeNormalWS,float planeEqWS, __global float4* ppVtxOut)\n" +"{\n" +" \n" +" int ve;\n" +" float ds, de;\n" +" int numVertsOut = 0;\n" +" //double-check next test\n" +" if (numVertsIn < 2)\n" +" return 0;\n" +" \n" +" float4 firstVertex=pVtxIn[numVertsIn-1];\n" +" float4 endVertex = pVtxIn[0];\n" +" \n" +" ds = dot3F4(planeNormalWS,firstVertex)+planeEqWS;\n" +" \n" +" for (ve = 0; ve < numVertsIn; ve++)\n" +" {\n" +" endVertex=pVtxIn[ve];\n" +" de = dot3F4(planeNormalWS,endVertex)+planeEqWS;\n" +" if (ds<0)\n" +" {\n" +" if (de<0)\n" +" {\n" +" // Start < 0, end < 0, so output endVertex\n" +" ppVtxOut[numVertsOut++] = endVertex;\n" +" }\n" +" else\n" +" {\n" +" // Start < 0, end >= 0, so output intersection\n" +" ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );\n" +" }\n" +" }\n" +" else\n" +" {\n" +" if (de<0)\n" +" {\n" +" // Start >= 0, end < 0 so output intersection and end\n" +" ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );\n" +" ppVtxOut[numVertsOut++] = endVertex;\n" +" }\n" +" }\n" +" firstVertex = endVertex;\n" +" ds = de;\n" +" }\n" +" return numVertsOut;\n" +"}\n" +"// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut\n" +"int clipFace(const float4* pVtxIn, int numVertsIn, float4 planeNormalWS,float planeEqWS, float4* ppVtxOut)\n" +"{\n" +" \n" +" int ve;\n" +" float ds, de;\n" +" int numVertsOut = 0;\n" +"//double-check next test\n" +" if (numVertsIn < 2)\n" +" return 0;\n" +" float4 firstVertex=pVtxIn[numVertsIn-1];\n" +" float4 endVertex = pVtxIn[0];\n" +" \n" +" ds = dot3F4(planeNormalWS,firstVertex)+planeEqWS;\n" +" for (ve = 0; ve < numVertsIn; ve++)\n" +" {\n" +" endVertex=pVtxIn[ve];\n" +" de = dot3F4(planeNormalWS,endVertex)+planeEqWS;\n" +" if (ds<0)\n" +" {\n" +" if (de<0)\n" +" {\n" +" // Start < 0, end < 0, so output endVertex\n" +" ppVtxOut[numVertsOut++] = endVertex;\n" +" }\n" +" else\n" +" {\n" +" // Start < 0, end >= 0, so output intersection\n" +" ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );\n" +" }\n" +" }\n" +" else\n" +" {\n" +" if (de<0)\n" +" {\n" +" // Start >= 0, end < 0 so output intersection and end\n" +" ppVtxOut[numVertsOut++] = lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) );\n" +" ppVtxOut[numVertsOut++] = endVertex;\n" +" }\n" +" }\n" +" firstVertex = endVertex;\n" +" ds = de;\n" +" }\n" +" return numVertsOut;\n" +"}\n" +"int clipFaceAgainstHull(const float4 separatingNormal, __global const b3ConvexPolyhedronData_t* hullA, \n" +" const float4 posA, const Quaternion ornA, float4* worldVertsB1, int numWorldVertsB1,\n" +" float4* worldVertsB2, int capacityWorldVertsB2,\n" +" const float minDist, float maxDist,\n" +" __global const float4* vertices,\n" +" __global const b3GpuFace_t* faces,\n" +" __global const int* indices,\n" +" float4* contactsOut,\n" +" int contactCapacity)\n" +"{\n" +" int numContactsOut = 0;\n" +" float4* pVtxIn = worldVertsB1;\n" +" float4* pVtxOut = worldVertsB2;\n" +" \n" +" int numVertsIn = numWorldVertsB1;\n" +" int numVertsOut = 0;\n" +" int closestFaceA=-1;\n" +" {\n" +" float dmin = FLT_MAX;\n" +" for(int face=0;facem_numFaces;face++)\n" +" {\n" +" const float4 Normal = make_float4(\n" +" faces[hullA->m_faceOffset+face].m_plane.x, \n" +" faces[hullA->m_faceOffset+face].m_plane.y, \n" +" faces[hullA->m_faceOffset+face].m_plane.z,0.f);\n" +" const float4 faceANormalWS = qtRotate(ornA,Normal);\n" +" \n" +" float d = dot3F4(faceANormalWS,separatingNormal);\n" +" if (d < dmin)\n" +" {\n" +" dmin = d;\n" +" closestFaceA = face;\n" +" }\n" +" }\n" +" }\n" +" if (closestFaceA<0)\n" +" return numContactsOut;\n" +" b3GpuFace_t polyA = faces[hullA->m_faceOffset+closestFaceA];\n" +" // clip polygon to back of planes of all faces of hull A that are adjacent to witness face\n" +" int numVerticesA = polyA.m_numIndices;\n" +" for(int e0=0;e0m_vertexOffset+indices[polyA.m_indexOffset+e0]];\n" +" const float4 b = vertices[hullA->m_vertexOffset+indices[polyA.m_indexOffset+((e0+1)%numVerticesA)]];\n" +" const float4 edge0 = a - b;\n" +" const float4 WorldEdge0 = qtRotate(ornA,edge0);\n" +" float4 planeNormalA = make_float4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);\n" +" float4 worldPlaneAnormal1 = qtRotate(ornA,planeNormalA);\n" +" float4 planeNormalWS1 = -cross3(WorldEdge0,worldPlaneAnormal1);\n" +" float4 worldA1 = transform(&a,&posA,&ornA);\n" +" float planeEqWS1 = -dot3F4(worldA1,planeNormalWS1);\n" +" \n" +" float4 planeNormalWS = planeNormalWS1;\n" +" float planeEqWS=planeEqWS1;\n" +" \n" +" //clip face\n" +" //clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);\n" +" numVertsOut = clipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut);\n" +" //btSwap(pVtxIn,pVtxOut);\n" +" float4* tmp = pVtxOut;\n" +" pVtxOut = pVtxIn;\n" +" pVtxIn = tmp;\n" +" numVertsIn = numVertsOut;\n" +" numVertsOut = 0;\n" +" }\n" +" \n" +" // only keep points that are behind the witness face\n" +" {\n" +" float4 localPlaneNormal = make_float4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);\n" +" float localPlaneEq = polyA.m_plane.w;\n" +" float4 planeNormalWS = qtRotate(ornA,localPlaneNormal);\n" +" float planeEqWS=localPlaneEq-dot3F4(planeNormalWS,posA);\n" +" for (int i=0;im_numFaces;face++)\n" +" {\n" +" const float4 Normal = make_float4(\n" +" facesA[hullA->m_faceOffset+face].m_plane.x, \n" +" facesA[hullA->m_faceOffset+face].m_plane.y, \n" +" facesA[hullA->m_faceOffset+face].m_plane.z,0.f);\n" +" const float4 faceANormalWS = qtRotate(ornA,Normal);\n" +" \n" +" float d = dot3F4(faceANormalWS,separatingNormal);\n" +" if (d < dmin)\n" +" {\n" +" dmin = d;\n" +" closestFaceA = face;\n" +" }\n" +" }\n" +" }\n" +" if (closestFaceA<0)\n" +" return numContactsOut;\n" +" b3GpuFace_t polyA = facesA[hullA->m_faceOffset+closestFaceA];\n" +" // clip polygon to back of planes of all faces of hull A that are adjacent to witness face\n" +" int numVerticesA = polyA.m_numIndices;\n" +" for(int e0=0;e0m_vertexOffset+indicesA[polyA.m_indexOffset+e0]];\n" +" const float4 b = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+((e0+1)%numVerticesA)]];\n" +" const float4 edge0 = a - b;\n" +" const float4 WorldEdge0 = qtRotate(ornA,edge0);\n" +" float4 planeNormalA = make_float4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);\n" +" float4 worldPlaneAnormal1 = qtRotate(ornA,planeNormalA);\n" +" float4 planeNormalWS1 = -cross3(WorldEdge0,worldPlaneAnormal1);\n" +" float4 worldA1 = transform(&a,&posA,&ornA);\n" +" float planeEqWS1 = -dot3F4(worldA1,planeNormalWS1);\n" +" \n" +" float4 planeNormalWS = planeNormalWS1;\n" +" float planeEqWS=planeEqWS1;\n" +" \n" +" //clip face\n" +" //clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);\n" +" numVertsOut = clipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut);\n" +" //btSwap(pVtxIn,pVtxOut);\n" +" float4* tmp = pVtxOut;\n" +" pVtxOut = pVtxIn;\n" +" pVtxIn = tmp;\n" +" numVertsIn = numVertsOut;\n" +" numVertsOut = 0;\n" +" }\n" +" \n" +" // only keep points that are behind the witness face\n" +" {\n" +" float4 localPlaneNormal = make_float4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f);\n" +" float localPlaneEq = polyA.m_plane.w;\n" +" float4 planeNormalWS = qtRotate(ornA,localPlaneNormal);\n" +" float planeEqWS=localPlaneEq-dot3F4(planeNormalWS,posA);\n" +" for (int i=0;im_numFaces;face++)\n" +" {\n" +" const float4 Normal = make_float4(faces[hullB->m_faceOffset+face].m_plane.x, \n" +" faces[hullB->m_faceOffset+face].m_plane.y, faces[hullB->m_faceOffset+face].m_plane.z,0.f);\n" +" const float4 WorldNormal = qtRotate(ornB, Normal);\n" +" float d = dot3F4(WorldNormal,separatingNormal);\n" +" if (d > dmax)\n" +" {\n" +" dmax = d;\n" +" closestFaceB = face;\n" +" }\n" +" }\n" +" }\n" +" {\n" +" const b3GpuFace_t polyB = faces[hullB->m_faceOffset+closestFaceB];\n" +" const int numVertices = polyB.m_numIndices;\n" +" for(int e0=0;e0m_vertexOffset+indices[polyB.m_indexOffset+e0]];\n" +" worldVertsB1[numWorldVertsB1++] = transform(&b,&posB,&ornB);\n" +" }\n" +" }\n" +" if (closestFaceB>=0)\n" +" {\n" +" numContactsOut = clipFaceAgainstHull(separatingNormal, hullA, \n" +" posA,ornA,\n" +" worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist,vertices,\n" +" faces,\n" +" indices,localContactsOut,localContactCapacity);\n" +" }\n" +" return numContactsOut;\n" +"}\n" +"int clipHullAgainstHullLocalA(const float4 separatingNormal,\n" +" const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB, \n" +" const float4 posA, const Quaternion ornA,const float4 posB, const Quaternion ornB, \n" +" float4* worldVertsB1, float4* worldVertsB2, int capacityWorldVerts,\n" +" const float minDist, float maxDist,\n" +" const float4* verticesA,\n" +" const b3GpuFace_t* facesA,\n" +" const int* indicesA,\n" +" __global const float4* verticesB,\n" +" __global const b3GpuFace_t* facesB,\n" +" __global const int* indicesB,\n" +" float4* localContactsOut,\n" +" int localContactCapacity)\n" +"{\n" +" int numContactsOut = 0;\n" +" int numWorldVertsB1= 0;\n" +" int closestFaceB=-1;\n" +" float dmax = -FLT_MAX;\n" +" {\n" +" for(int face=0;facem_numFaces;face++)\n" +" {\n" +" const float4 Normal = make_float4(facesB[hullB->m_faceOffset+face].m_plane.x, \n" +" facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f);\n" +" const float4 WorldNormal = qtRotate(ornB, Normal);\n" +" float d = dot3F4(WorldNormal,separatingNormal);\n" +" if (d > dmax)\n" +" {\n" +" dmax = d;\n" +" closestFaceB = face;\n" +" }\n" +" }\n" +" }\n" +" {\n" +" const b3GpuFace_t polyB = facesB[hullB->m_faceOffset+closestFaceB];\n" +" const int numVertices = polyB.m_numIndices;\n" +" for(int e0=0;e0m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];\n" +" worldVertsB1[numWorldVertsB1++] = transform(&b,&posB,&ornB);\n" +" }\n" +" }\n" +" if (closestFaceB>=0)\n" +" {\n" +" numContactsOut = clipFaceAgainstHullLocalA(separatingNormal, hullA, \n" +" posA,ornA,\n" +" worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist,\n" +" verticesA,facesA,indicesA,\n" +" verticesB,facesB,indicesB,\n" +" localContactsOut,localContactCapacity);\n" +" }\n" +" return numContactsOut;\n" +"}\n" +"#define PARALLEL_SUM(v, n) for(int j=1; j v[i+offset].y)? v[i]: v[i+offset]; }\n" +"#define REDUCE_MIN(v, n) {int i=0; for(int offset=0; offset64)\n" +" nPoints = 64;\n" +" \n" +" float4 center = make_float4(0.f);\n" +" {\n" +" \n" +" for (int i=0;i a[ie].x )? a[0].x: a[ie].x;\n" +" a[0].y = (a[0].y > a[ie].y )? a[0].y: a[ie].y;\n" +" a[0].z = (a[0].z > a[ie].z )? a[0].z: a[ie].z;\n" +" a[0].w = (a[0].w > a[ie].w )? a[0].w: a[ie].w;\n" +" }\n" +" idx[0] = (int)a[0].x & 0xff;\n" +" idx[1] = (int)a[0].y & 0xff;\n" +" idx[2] = (int)a[0].z & 0xff;\n" +" idx[3] = (int)a[0].w & 0xff;\n" +" }\n" +" }\n" +" {\n" +" float2 h[64];\n" +" PARALLEL_DO( h[ie] = make_float2((float)ie, p[ie].w), nPoints );\n" +" REDUCE_MIN( h, nPoints );\n" +" max00 = h[0];\n" +" }\n" +" }\n" +" contactIdx[0] = idx[0];\n" +" contactIdx[1] = idx[1];\n" +" contactIdx[2] = idx[2];\n" +" contactIdx[3] = idx[3];\n" +" return 4;\n" +" }\n" +"}\n" +"__kernel void extractManifoldAndAddContactKernel(__global const int4* pairs, \n" +" __global const b3RigidBodyData_t* rigidBodies, \n" +" __global const float4* closestPointsWorld,\n" +" __global const float4* separatingNormalsWorld,\n" +" __global const int* contactCounts,\n" +" __global const int* contactOffsets,\n" +" __global struct b3Contact4Data* restrict contactsOut,\n" +" counter32_t nContactsOut,\n" +" int contactCapacity,\n" +" int numPairs,\n" +" int pairIndex\n" +" )\n" +"{\n" +" int idx = get_global_id(0);\n" +" \n" +" if (idxm_worldNormalOnB = -normal;\n" +" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" +" c->m_batchIdx = idx;\n" +" int bodyA = pairs[pairIndex].x;\n" +" int bodyB = pairs[pairIndex].y;\n" +" c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0 ? -bodyA:bodyA;\n" +" c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0 ? -bodyB:bodyB;\n" +" c->m_childIndexA = -1;\n" +" c->m_childIndexB = -1;\n" +" for (int i=0;im_worldPosB[i] = localPoints[contactIdx[i]];\n" +" }\n" +" GET_NPOINTS(*c) = nContacts;\n" +" }\n" +" }\n" +"}\n" +"void trInverse(float4 translationIn, Quaternion orientationIn,\n" +" float4* translationOut, Quaternion* orientationOut)\n" +"{\n" +" *orientationOut = qtInvert(orientationIn);\n" +" *translationOut = qtRotate(*orientationOut, -translationIn);\n" +"}\n" +"void trMul(float4 translationA, Quaternion orientationA,\n" +" float4 translationB, Quaternion orientationB,\n" +" float4* translationOut, Quaternion* orientationOut)\n" +"{\n" +" *orientationOut = qtMul(orientationA,orientationB);\n" +" *translationOut = transform(&translationB,&translationA,&orientationA);\n" +"}\n" +"__kernel void clipHullHullKernel( __global int4* pairs, \n" +" __global const b3RigidBodyData_t* rigidBodies, \n" +" __global const b3Collidable_t* collidables,\n" +" __global const b3ConvexPolyhedronData_t* convexShapes, \n" +" __global const float4* vertices,\n" +" __global const float4* uniqueEdges,\n" +" __global const b3GpuFace_t* faces,\n" +" __global const int* indices,\n" +" __global const float4* separatingNormals,\n" +" __global const int* hasSeparatingAxis,\n" +" __global struct b3Contact4Data* restrict globalContactsOut,\n" +" counter32_t nGlobalContactsOut,\n" +" int numPairs,\n" +" int contactCapacity)\n" +"{\n" +" int i = get_global_id(0);\n" +" int pairIndex = i;\n" +" \n" +" float4 worldVertsB1[64];\n" +" float4 worldVertsB2[64];\n" +" int capacityWorldVerts = 64; \n" +" float4 localContactsOut[64];\n" +" int localContactCapacity=64;\n" +" \n" +" float minDist = -1e30f;\n" +" float maxDist = 0.02f;\n" +" if (i0)\n" +" {\n" +" float4 normal = -separatingNormals[i];\n" +" int nPoints = numLocalContactsOut;\n" +" float4* pointsIn = localContactsOut;\n" +" int contactIdx[4];// = {-1,-1,-1,-1};\n" +" contactIdx[0] = -1;\n" +" contactIdx[1] = -1;\n" +" contactIdx[2] = -1;\n" +" contactIdx[3] = -1;\n" +" \n" +" int nReducedContacts = extractManifoldSequential(pointsIn, nPoints, normal, contactIdx);\n" +" \n" +" \n" +" int mprContactIndex = pairs[pairIndex].z;\n" +" int dstIdx = mprContactIndex;\n" +" if (dstIdx<0)\n" +" {\n" +" AppendInc( nGlobalContactsOut, dstIdx );\n" +" }\n" +" if (dstIdxm_worldNormalOnB = -normal;\n" +" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" +" c->m_batchIdx = pairIndex;\n" +" int bodyA = pairs[pairIndex].x;\n" +" int bodyB = pairs[pairIndex].y;\n" +" c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;\n" +" c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;\n" +" c->m_childIndexA = -1;\n" +" c->m_childIndexB = -1;\n" +" for (int i=0;i0||(mprContactIndex<0))\n" +" {\n" +" c->m_worldPosB[i] = pointsIn[contactIdx[i]];\n" +" }\n" +" }\n" +" GET_NPOINTS(*c) = nReducedContacts;\n" +" }\n" +" \n" +" }// if (numContactsOut>0)\n" +" }// if (hasSeparatingAxis[i])\n" +" }// if (i= 0)\n" +" {\n" +" collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;\n" +" float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;\n" +" float4 childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;\n" +" float4 newPosA = qtRotate(ornA,childPosA)+posA;\n" +" float4 newOrnA = qtMul(ornA,childOrnA);\n" +" posA = newPosA;\n" +" ornA = newOrnA;\n" +" } else\n" +" {\n" +" collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" +" }\n" +" \n" +" if (childShapeIndexB>=0)\n" +" {\n" +" collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;\n" +" float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;\n" +" float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;\n" +" float4 newPosB = transform(&childPosB,&posB,&ornB);\n" +" float4 newOrnB = qtMul(ornB,childOrnB);\n" +" posB = newPosB;\n" +" ornB = newOrnB;\n" +" } else\n" +" {\n" +" collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; \n" +" }\n" +" \n" +" int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" +" int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" +" \n" +" int numLocalContactsOut = clipHullAgainstHull(gpuCompoundSepNormalsOut[i],\n" +" &convexShapes[shapeIndexA], &convexShapes[shapeIndexB],\n" +" posA,ornA,\n" +" posB,ornB,\n" +" worldVertsB1,worldVertsB2,capacityWorldVerts,\n" +" minDist, maxDist,\n" +" vertices,faces,indices,\n" +" localContactsOut,localContactCapacity);\n" +" \n" +" if (numLocalContactsOut>0)\n" +" {\n" +" float4 normal = -gpuCompoundSepNormalsOut[i];\n" +" int nPoints = numLocalContactsOut;\n" +" float4* pointsIn = localContactsOut;\n" +" int contactIdx[4];// = {-1,-1,-1,-1};\n" +" contactIdx[0] = -1;\n" +" contactIdx[1] = -1;\n" +" contactIdx[2] = -1;\n" +" contactIdx[3] = -1;\n" +" \n" +" int nReducedContacts = extractManifoldSequential(pointsIn, nPoints, normal, contactIdx);\n" +" \n" +" int dstIdx;\n" +" AppendInc( nGlobalContactsOut, dstIdx );\n" +" if ((dstIdx+nReducedContacts) < maxContactCapacity)\n" +" {\n" +" __global struct b3Contact4Data* c = globalContactsOut+ dstIdx;\n" +" c->m_worldNormalOnB = -normal;\n" +" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" +" c->m_batchIdx = pairIndex;\n" +" int bodyA = gpuCompoundPairs[pairIndex].x;\n" +" int bodyB = gpuCompoundPairs[pairIndex].y;\n" +" c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;\n" +" c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;\n" +" c->m_childIndexA = childShapeIndexA;\n" +" c->m_childIndexB = childShapeIndexB;\n" +" for (int i=0;im_worldPosB[i] = pointsIn[contactIdx[i]];\n" +" }\n" +" GET_NPOINTS(*c) = nReducedContacts;\n" +" }\n" +" \n" +" }// if (numContactsOut>0)\n" +" }// if (gpuHasCompoundSepNormalsOut[i])\n" +" }// if (i 0.00001)\n" +" {\n" +" normalOnSurfaceB = diff / len;\n" +" }\n" +" float4 contactPosB = posB + normalOnSurfaceB*radiusB;\n" +" contactPosB.w = dist;\n" +" \n" +" int dstIdx;\n" +" AppendInc( nGlobalContactsOut, dstIdx );\n" +" if (dstIdx < contactCapacity)\n" +" {\n" +" __global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n" +" c->m_worldNormalOnB = -normalOnSurfaceB;\n" +" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" +" c->m_batchIdx = pairIndex;\n" +" int bodyA = pairs[pairIndex].x;\n" +" int bodyB = pairs[pairIndex].y;\n" +" c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;\n" +" c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;\n" +" c->m_worldPosB[0] = contactPosB;\n" +" c->m_childIndexA = -1;\n" +" c->m_childIndexB = -1;\n" +" GET_NPOINTS(*c) = 1;\n" +" }//if (dstIdx < numPairs)\n" +" }//if ( len <= (radiusA+radiusB))\n" +" }//SHAPE_SPHERE SHAPE_SPHERE\n" +" }//if (i0)\n" +" {\n" +" float4 normal = -separatingNormals[i];\n" +" int nPoints = numLocalContactsOut;\n" +" float4* pointsIn = localContactsOut;\n" +" int contactIdx[4];// = {-1,-1,-1,-1};\n" +" contactIdx[0] = -1;\n" +" contactIdx[1] = -1;\n" +" contactIdx[2] = -1;\n" +" contactIdx[3] = -1;\n" +" \n" +" int nReducedContacts = extractManifoldSequential(pointsIn, nPoints, normal, contactIdx);\n" +" \n" +" int dstIdx;\n" +" AppendInc( nGlobalContactsOut, dstIdx );\n" +" if (dstIdxm_worldNormalOnB = -normal;\n" +" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" +" c->m_batchIdx = pairIndex;\n" +" int bodyA = concavePairsIn[pairIndex].x;\n" +" int bodyB = concavePairsIn[pairIndex].y;\n" +" c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;\n" +" c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;\n" +" c->m_childIndexA = childShapeIndexA;\n" +" c->m_childIndexB = childShapeIndexB;\n" +" for (int i=0;im_worldPosB[i] = pointsIn[contactIdx[i]];\n" +" }\n" +" GET_NPOINTS(*c) = nReducedContacts;\n" +" }\n" +" \n" +" }// if (numContactsOut>0)\n" +" }// if (im_numFaces;face++)\n" +" {\n" +" const float4 Normal = make_float4(faces[hullB->m_faceOffset+face].m_plane.x,\n" +" faces[hullB->m_faceOffset+face].m_plane.y, faces[hullB->m_faceOffset+face].m_plane.z,0.f);\n" +" const float4 WorldNormal = qtRotate(ornB, Normal);\n" +" float d = dot3F4(WorldNormal,separatingNormal);\n" +" if (d > dmax)\n" +" {\n" +" dmax = d;\n" +" closestFaceB = face;\n" +" }\n" +" }\n" +" }\n" +" \n" +" {\n" +" const b3GpuFace_t polyB = faces[hullB->m_faceOffset+closestFaceB];\n" +" const int numVertices = polyB.m_numIndices;\n" +" for(int e0=0;e0m_vertexOffset+indices[polyB.m_indexOffset+e0]];\n" +" worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = transform(&b,&posB,&ornB);\n" +" }\n" +" }\n" +" \n" +" int closestFaceA=-1;\n" +" {\n" +" float dmin = FLT_MAX;\n" +" for(int face=0;facem_numFaces;face++)\n" +" {\n" +" const float4 Normal = make_float4(\n" +" faces[hullA->m_faceOffset+face].m_plane.x,\n" +" faces[hullA->m_faceOffset+face].m_plane.y,\n" +" faces[hullA->m_faceOffset+face].m_plane.z,\n" +" 0.f);\n" +" const float4 faceANormalWS = qtRotate(ornA,Normal);\n" +" \n" +" float d = dot3F4(faceANormalWS,separatingNormal);\n" +" if (d < dmin)\n" +" {\n" +" dmin = d;\n" +" closestFaceA = face;\n" +" worldNormalsA1[pairIndex] = faceANormalWS;\n" +" }\n" +" }\n" +" }\n" +" \n" +" int numVerticesA = faces[hullA->m_faceOffset+closestFaceA].m_numIndices;\n" +" for(int e0=0;e0m_vertexOffset+indices[faces[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]];\n" +" worldVertsA1[pairIndex*capacityWorldVerts+e0] = transform(&a, &posA,&ornA);\n" +" }\n" +" \n" +" clippingFaces[pairIndex].x = closestFaceA;\n" +" clippingFaces[pairIndex].y = closestFaceB;\n" +" clippingFaces[pairIndex].z = numVerticesA;\n" +" clippingFaces[pairIndex].w = numWorldVertsB1;\n" +" \n" +" \n" +" return numContactsOut;\n" +"}\n" +"int clipFaces(__global float4* worldVertsA1,\n" +" __global float4* worldNormalsA1,\n" +" __global float4* worldVertsB1,\n" +" __global float4* worldVertsB2, \n" +" int capacityWorldVertsB2,\n" +" const float minDist, float maxDist,\n" +" __global int4* clippingFaces,\n" +" int pairIndex)\n" +"{\n" +" int numContactsOut = 0;\n" +" \n" +" int closestFaceA = clippingFaces[pairIndex].x;\n" +" int closestFaceB = clippingFaces[pairIndex].y;\n" +" int numVertsInA = clippingFaces[pairIndex].z;\n" +" int numVertsInB = clippingFaces[pairIndex].w;\n" +" \n" +" int numVertsOut = 0;\n" +" \n" +" if (closestFaceA<0)\n" +" return numContactsOut;\n" +" \n" +" __global float4* pVtxIn = &worldVertsB1[pairIndex*capacityWorldVertsB2];\n" +" __global float4* pVtxOut = &worldVertsB2[pairIndex*capacityWorldVertsB2];\n" +" \n" +" \n" +" \n" +" // clip polygon to back of planes of all faces of hull A that are adjacent to witness face\n" +" \n" +" for(int e0=0;e0=0)\n" +" {\n" +" \n" +" \n" +" \n" +" // clip polygon to back of planes of all faces of hull A that are adjacent to witness face\n" +" \n" +" for(int e0=0;e00)\n" +" {\n" +" __global float4* pointsIn = &worldVertsB2[pairIndex*vertexFaceCapacity];\n" +" float4 normal = -separatingNormals[i];\n" +" \n" +" int nReducedContacts = extractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);\n" +" \n" +" int mprContactIndex = pairs[pairIndex].z;\n" +" int dstIdx = mprContactIndex;\n" +" if (dstIdx<0)\n" +" {\n" +" AppendInc( nGlobalContactsOut, dstIdx );\n" +" }\n" +"//#if 0\n" +" \n" +" if (dstIdx < contactCapacity)\n" +" {\n" +" __global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n" +" c->m_worldNormalOnB = -normal;\n" +" c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n" +" c->m_batchIdx = pairIndex;\n" +" int bodyA = pairs[pairIndex].x;\n" +" int bodyB = pairs[pairIndex].y;\n" +" pairs[pairIndex].w = dstIdx;\n" +" c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;\n" +" c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;\n" +" c->m_childIndexA =-1;\n" +" c->m_childIndexB =-1;\n" +" switch (nReducedContacts)\n" +" {\n" +" case 4:\n" +" c->m_worldPosB[3] = pointsIn[contactIdx.w];\n" +" case 3:\n" +" c->m_worldPosB[2] = pointsIn[contactIdx.z];\n" +" case 2:\n" +" c->m_worldPosB[1] = pointsIn[contactIdx.y];\n" +" case 1:\n" +" if (mprContactIndex<0)//test\n" +" c->m_worldPosB[0] = pointsIn[contactIdx.x];\n" +" default:\n" +" {\n" +" }\n" +" };\n" +" \n" +" GET_NPOINTS(*c) = nReducedContacts;\n" +" \n" +" }\n" +" \n" +" \n" +"//#endif\n" +" \n" +" }// if (numContactsOut>0)\n" +" }// if (hasSeparatingAxis[i])\n" +" }// if (im_escapeIndexOrTriangleIndex&~(y)); +} + +int getTriangleIndexGlobal(__global const b3QuantizedBvhNode* rootNode) +{ + unsigned int x=0; + unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS); + // Get only the lower bits where the triangle index is stored + return (rootNode->m_escapeIndexOrTriangleIndex&~(y)); +} + +int isLeafNode(const b3QuantizedBvhNode* rootNode) +{ + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0; +} + +int isLeafNodeGlobal(__global const b3QuantizedBvhNode* rootNode) +{ + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0; +} + +int getEscapeIndex(const b3QuantizedBvhNode* rootNode) +{ + return -rootNode->m_escapeIndexOrTriangleIndex; +} + +int getEscapeIndexGlobal(__global const b3QuantizedBvhNode* rootNode) +{ + return -rootNode->m_escapeIndexOrTriangleIndex; +} + + +typedef struct +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes, points to the root of the subtree + int m_rootNodeIndex; + //4 bytes + int m_subtreeSize; + int m_padding[3]; +} b3BvhSubtreeInfo; + + + + + + + +typedef struct +{ + float4 m_childPosition; + float4 m_childOrientation; + int m_shapeIndex; + int m_unused0; + int m_unused1; + int m_unused2; +} btGpuChildShape; + + +typedef struct +{ + float4 m_pos; + float4 m_quat; + float4 m_linVel; + float4 m_angVel; + + u32 m_collidableIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} BodyData; + + +typedef struct +{ + float4 m_localCenter; + float4 m_extents; + float4 mC; + float4 mE; + + float m_radius; + int m_faceOffset; + int m_numFaces; + int m_numVertices; + + int m_vertexOffset; + int m_uniqueEdgesOffset; + int m_numUniqueEdges; + int m_unused; +} ConvexPolyhedronCL; + +typedef struct +{ + union + { + float4 m_min; + float m_minElems[4]; + int m_minIndices[4]; + }; + union + { + float4 m_max; + float m_maxElems[4]; + int m_maxIndices[4]; + }; +} btAabbCL; + +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Common/shared/b3Int2.h" + + + +typedef struct +{ + float4 m_plane; + int m_indexOffset; + int m_numIndices; +} btGpuFace; + +#define make_float4 (float4) + + +__inline +float4 cross3(float4 a, float4 b) +{ + return cross(a,b); + + +// float4 a1 = make_float4(a.xyz,0.f); +// float4 b1 = make_float4(b.xyz,0.f); + +// return cross(a1,b1); + +//float4 c = make_float4(a.y*b.z - a.z*b.y,a.z*b.x - a.x*b.z,a.x*b.y - a.y*b.x,0.f); + + // float4 c = make_float4(a.y*b.z - a.z*b.y,1.f,a.x*b.y - a.y*b.x,0.f); + + //return c; +} + +__inline +float dot3F4(float4 a, float4 b) +{ + float4 a1 = make_float4(a.xyz,0.f); + float4 b1 = make_float4(b.xyz,0.f); + return dot(a1, b1); +} + +__inline +float4 fastNormalize4(float4 v) +{ + v = make_float4(v.xyz,0.f); + return fast_normalize(v); +} + + +/////////////////////////////////////// +// Quaternion +/////////////////////////////////////// + +typedef float4 Quaternion; + +__inline +Quaternion qtMul(Quaternion a, Quaternion b); + +__inline +Quaternion qtNormalize(Quaternion in); + +__inline +float4 qtRotate(Quaternion q, float4 vec); + +__inline +Quaternion qtInvert(Quaternion q); + + + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b) +{ + Quaternion ans; + ans = cross3( a, b ); + ans += a.w*b+b.w*a; +// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - dot3F4(a, b); + return ans; +} + +__inline +Quaternion qtNormalize(Quaternion in) +{ + return fastNormalize4(in); +// in /= length( in ); +// return in; +} +__inline +float4 qtRotate(Quaternion q, float4 vec) +{ + Quaternion qInv = qtInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = qtMul(qtMul(q,vcpy),qInv); + return out; +} + +__inline +Quaternion qtInvert(Quaternion q) +{ + return (Quaternion)(-q.xyz, q.w); +} + +__inline +float4 qtInvRotate(const Quaternion q, float4 vec) +{ + return qtRotate( qtInvert( q ), vec ); +} + +__inline +float4 transform(const float4* p, const float4* translation, const Quaternion* orientation) +{ + return qtRotate( *orientation, *p ) + (*translation); +} + + + +__inline +float4 normalize3(const float4 a) +{ + float4 n = make_float4(a.x, a.y, a.z, 0.f); + return fastNormalize4( n ); +} + +inline void projectLocal(const ConvexPolyhedronCL* hull, const float4 pos, const float4 orn, +const float4* dir, const float4* vertices, float* min, float* max) +{ + min[0] = FLT_MAX; + max[0] = -FLT_MAX; + int numVerts = hull->m_numVertices; + + const float4 localDir = qtInvRotate(orn,*dir); + float offset = dot(pos,*dir); + for(int i=0;im_vertexOffset+i],localDir); + if(dp < min[0]) + min[0] = dp; + if(dp > max[0]) + max[0] = dp; + } + if(min[0]>max[0]) + { + float tmp = min[0]; + min[0] = max[0]; + max[0] = tmp; + } + min[0] += offset; + max[0] += offset; +} + +inline void project(__global const ConvexPolyhedronCL* hull, const float4 pos, const float4 orn, +const float4* dir, __global const float4* vertices, float* min, float* max) +{ + min[0] = FLT_MAX; + max[0] = -FLT_MAX; + int numVerts = hull->m_numVertices; + + const float4 localDir = qtInvRotate(orn,*dir); + float offset = dot(pos,*dir); + for(int i=0;im_vertexOffset+i],localDir); + if(dp < min[0]) + min[0] = dp; + if(dp > max[0]) + max[0] = dp; + } + if(min[0]>max[0]) + { + float tmp = min[0]; + min[0] = max[0]; + max[0] = tmp; + } + min[0] += offset; + max[0] += offset; +} + +inline bool TestSepAxisLocalA(const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, + const float4 posA,const float4 ornA, + const float4 posB,const float4 ornB, + float4* sep_axis, const float4* verticesA, __global const float4* verticesB,float* depth) +{ + float Min0,Max0; + float Min1,Max1; + projectLocal(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0); + project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1); + + if(Max01e-6f || fabs(v.y)>1e-6f || fabs(v.z)>1e-6f) + return false; + return true; +} + + + +bool findSeparatingAxisLocalA( const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, + const float4 posA1, + const float4 ornA, + const float4 posB1, + const float4 ornB, + const float4 DeltaC2, + + const float4* verticesA, + const float4* uniqueEdgesA, + const btGpuFace* facesA, + const int* indicesA, + + __global const float4* verticesB, + __global const float4* uniqueEdgesB, + __global const btGpuFace* facesB, + __global const int* indicesB, + float4* sep, + float* dmin) +{ + + + float4 posA = posA1; + posA.w = 0.f; + float4 posB = posB1; + posB.w = 0.f; + int curPlaneTests=0; + { + int numFacesA = hullA->m_numFaces; + // Test normals from hullA + for(int i=0;im_faceOffset+i].m_plane; + float4 faceANormalWS = qtRotate(ornA,normal); + if (dot3F4(DeltaC2,faceANormalWS)<0) + faceANormalWS*=-1.f; + curPlaneTests++; + float d; + if(!TestSepAxisLocalA( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d)) + return false; + if(d<*dmin) + { + *dmin = d; + *sep = faceANormalWS; + } + } + } + if((dot3F4(-DeltaC2,*sep))>0.0f) + { + *sep = -(*sep); + } + return true; +} + +bool findSeparatingAxisLocalB( __global const ConvexPolyhedronCL* hullA, const ConvexPolyhedronCL* hullB, + const float4 posA1, + const float4 ornA, + const float4 posB1, + const float4 ornB, + const float4 DeltaC2, + __global const float4* verticesA, + __global const float4* uniqueEdgesA, + __global const btGpuFace* facesA, + __global const int* indicesA, + const float4* verticesB, + const float4* uniqueEdgesB, + const btGpuFace* facesB, + const int* indicesB, + float4* sep, + float* dmin) +{ + + + float4 posA = posA1; + posA.w = 0.f; + float4 posB = posB1; + posB.w = 0.f; + int curPlaneTests=0; + { + int numFacesA = hullA->m_numFaces; + // Test normals from hullA + for(int i=0;im_faceOffset+i].m_plane; + float4 faceANormalWS = qtRotate(ornA,normal); + if (dot3F4(DeltaC2,faceANormalWS)<0) + faceANormalWS *= -1.f; + curPlaneTests++; + float d; + if(!TestSepAxisLocalA( hullB, hullA, posB,ornB,posA,ornA, &faceANormalWS, verticesB,verticesA, &d)) + return false; + if(d<*dmin) + { + *dmin = d; + *sep = faceANormalWS; + } + } + } + if((dot3F4(-DeltaC2,*sep))>0.0f) + { + *sep = -(*sep); + } + return true; +} + + + +bool findSeparatingAxisEdgeEdgeLocalA( const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, + const float4 posA1, + const float4 ornA, + const float4 posB1, + const float4 ornB, + const float4 DeltaC2, + const float4* verticesA, + const float4* uniqueEdgesA, + const btGpuFace* facesA, + const int* indicesA, + __global const float4* verticesB, + __global const float4* uniqueEdgesB, + __global const btGpuFace* facesB, + __global const int* indicesB, + float4* sep, + float* dmin) +{ + + + float4 posA = posA1; + posA.w = 0.f; + float4 posB = posB1; + posB.w = 0.f; + + int curPlaneTests=0; + + int curEdgeEdge = 0; + // Test edges + for(int e0=0;e0m_numUniqueEdges;e0++) + { + const float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0]; + float4 edge0World = qtRotate(ornA,edge0); + + for(int e1=0;e1m_numUniqueEdges;e1++) + { + const float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1]; + float4 edge1World = qtRotate(ornB,edge1); + + + float4 crossje = cross3(edge0World,edge1World); + + curEdgeEdge++; + if(!IsAlmostZero(crossje)) + { + crossje = normalize3(crossje); + if (dot3F4(DeltaC2,crossje)<0) + crossje *= -1.f; + + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + projectLocal(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); + project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); + + if(Max00.0f) + { + *sep = -(*sep); + } + return true; +} + + + +inline int findClippingFaces(const float4 separatingNormal, + const ConvexPolyhedronCL* hullA, + __global const ConvexPolyhedronCL* hullB, + const float4 posA, const Quaternion ornA,const float4 posB, const Quaternion ornB, + __global float4* worldVertsA1, + __global float4* worldNormalsA1, + __global float4* worldVertsB1, + int capacityWorldVerts, + const float minDist, float maxDist, + const float4* verticesA, + const btGpuFace* facesA, + const int* indicesA, + __global const float4* verticesB, + __global const btGpuFace* facesB, + __global const int* indicesB, + __global int4* clippingFaces, int pairIndex) +{ + int numContactsOut = 0; + int numWorldVertsB1= 0; + + + int closestFaceB=0; + float dmax = -FLT_MAX; + + { + for(int face=0;facem_numFaces;face++) + { + const float4 Normal = make_float4(facesB[hullB->m_faceOffset+face].m_plane.x, + facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f); + const float4 WorldNormal = qtRotate(ornB, Normal); + float d = dot3F4(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + + { + const btGpuFace polyB = facesB[hullB->m_faceOffset+closestFaceB]; + int numVertices = polyB.m_numIndices; + if (numVertices>capacityWorldVerts) + numVertices = capacityWorldVerts; + if (numVertices<0) + numVertices = 0; + + for(int e0=0;e0m_vertexOffset+indicesB[polyB.m_indexOffset+e0]]; + worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = transform(&b,&posB,&ornB); + } + } + } + + int closestFaceA=0; + { + float dmin = FLT_MAX; + for(int face=0;facem_numFaces;face++) + { + const float4 Normal = make_float4( + facesA[hullA->m_faceOffset+face].m_plane.x, + facesA[hullA->m_faceOffset+face].m_plane.y, + facesA[hullA->m_faceOffset+face].m_plane.z, + 0.f); + const float4 faceANormalWS = qtRotate(ornA,Normal); + + float d = dot3F4(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + worldNormalsA1[pairIndex] = faceANormalWS; + } + } + } + + int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices; + if (numVerticesA>capacityWorldVerts) + numVerticesA = capacityWorldVerts; + if (numVerticesA<0) + numVerticesA=0; + + for(int e0=0;e0m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]]; + worldVertsA1[pairIndex*capacityWorldVerts+e0] = transform(&a, &posA,&ornA); + } + } + + clippingFaces[pairIndex].x = closestFaceA; + clippingFaces[pairIndex].y = closestFaceB; + clippingFaces[pairIndex].z = numVerticesA; + clippingFaces[pairIndex].w = numWorldVertsB1; + + + return numContactsOut; +} + + + + +// work-in-progress +__kernel void findConcaveSeparatingAxisVertexFaceKernel( __global int4* concavePairs, + __global const BodyData* rigidBodies, + __global const btCollidableGpu* collidables, + __global const ConvexPolyhedronCL* convexShapes, + __global const float4* vertices, + __global const float4* uniqueEdges, + __global const btGpuFace* faces, + __global const int* indices, + __global const btGpuChildShape* gpuChildShapes, + __global btAabbCL* aabbs, + __global float4* concaveSeparatingNormalsOut, + __global int* concaveHasSeparatingNormals, + __global int4* clippingFacesOut, + __global float4* worldVertsA1GPU, + __global float4* worldNormalsAGPU, + __global float4* worldVertsB1GPU, + __global float* dmins, + int vertexFaceCapacity, + int numConcavePairs + ) +{ + + int i = get_global_id(0); + if (i>=numConcavePairs) + return; + + concaveHasSeparatingNormals[i] = 0; + + int pairIdx = i; + + int bodyIndexA = concavePairs[i].x; + int bodyIndexB = concavePairs[i].y; + + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&& + collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + concavePairs[pairIdx].w = -1; + return; + } + + + + int numFacesA = convexShapes[shapeIndexA].m_numFaces; + int numActualConcaveConvexTests = 0; + + int f = concavePairs[i].z; + + bool overlap = false; + + ConvexPolyhedronCL convexPolyhedronA; + + //add 3 vertices of the triangle + convexPolyhedronA.m_numVertices = 3; + convexPolyhedronA.m_vertexOffset = 0; + float4 localCenter = make_float4(0.f,0.f,0.f,0.f); + + btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f]; + float4 triMinAabb, triMaxAabb; + btAabbCL triAabb; + triAabb.m_min = make_float4(1e30f,1e30f,1e30f,0.f); + triAabb.m_max = make_float4(-1e30f,-1e30f,-1e30f,0.f); + + float4 verticesA[3]; + for (int i=0;i<3;i++) + { + int index = indices[face.m_indexOffset+i]; + float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index]; + verticesA[i] = vert; + localCenter += vert; + + triAabb.m_min = min(triAabb.m_min,vert); + triAabb.m_max = max(triAabb.m_max,vert); + + } + + overlap = true; + overlap = (triAabb.m_min.x > aabbs[bodyIndexB].m_max.x || triAabb.m_max.x < aabbs[bodyIndexB].m_min.x) ? false : overlap; + overlap = (triAabb.m_min.z > aabbs[bodyIndexB].m_max.z || triAabb.m_max.z < aabbs[bodyIndexB].m_min.z) ? false : overlap; + overlap = (triAabb.m_min.y > aabbs[bodyIndexB].m_max.y || triAabb.m_max.y < aabbs[bodyIndexB].m_min.y) ? false : overlap; + + if (overlap) + { + float dmin = FLT_MAX; + int hasSeparatingAxis=5; + float4 sepAxis=make_float4(1,2,3,4); + + int localCC=0; + numActualConcaveConvexTests++; + + //a triangle has 3 unique edges + convexPolyhedronA.m_numUniqueEdges = 3; + convexPolyhedronA.m_uniqueEdgesOffset = 0; + float4 uniqueEdgesA[3]; + + uniqueEdgesA[0] = (verticesA[1]-verticesA[0]); + uniqueEdgesA[1] = (verticesA[2]-verticesA[1]); + uniqueEdgesA[2] = (verticesA[0]-verticesA[2]); + + + convexPolyhedronA.m_faceOffset = 0; + + float4 normal = make_float4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); + + btGpuFace facesA[TRIANGLE_NUM_CONVEX_FACES]; + int indicesA[3+3+2+2+2]; + int curUsedIndices=0; + int fidx=0; + + //front size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[0] = 0; + indicesA[1] = 1; + indicesA[2] = 2; + curUsedIndices+=3; + float c = face.m_plane.w; + facesA[fidx].m_plane.x = normal.x; + facesA[fidx].m_plane.y = normal.y; + facesA[fidx].m_plane.z = normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + //back size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[3]=2; + indicesA[4]=1; + indicesA[5]=0; + curUsedIndices+=3; + float c = dot(normal,verticesA[0]); + float c1 = -face.m_plane.w; + facesA[fidx].m_plane.x = -normal.x; + facesA[fidx].m_plane.y = -normal.y; + facesA[fidx].m_plane.z = -normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + + bool addEdgePlanes = true; + if (addEdgePlanes) + { + int numVertices=3; + int prevVertex = numVertices-1; + for (int i=0;i=numConcavePairs) + return; + + if (!concaveHasSeparatingNormals[i]) + return; + + int pairIdx = i; + + int bodyIndexA = concavePairs[i].x; + int bodyIndexB = concavePairs[i].y; + + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + + int numFacesA = convexShapes[shapeIndexA].m_numFaces; + int numActualConcaveConvexTests = 0; + + int f = concavePairs[i].z; + + bool overlap = false; + + ConvexPolyhedronCL convexPolyhedronA; + + //add 3 vertices of the triangle + convexPolyhedronA.m_numVertices = 3; + convexPolyhedronA.m_vertexOffset = 0; + float4 localCenter = make_float4(0.f,0.f,0.f,0.f); + + btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f]; + float4 triMinAabb, triMaxAabb; + btAabbCL triAabb; + triAabb.m_min = make_float4(1e30f,1e30f,1e30f,0.f); + triAabb.m_max = make_float4(-1e30f,-1e30f,-1e30f,0.f); + + float4 verticesA[3]; + for (int i=0;i<3;i++) + { + int index = indices[face.m_indexOffset+i]; + float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index]; + verticesA[i] = vert; + localCenter += vert; + + triAabb.m_min = min(triAabb.m_min,vert); + triAabb.m_max = max(triAabb.m_max,vert); + + } + + overlap = true; + overlap = (triAabb.m_min.x > aabbs[bodyIndexB].m_max.x || triAabb.m_max.x < aabbs[bodyIndexB].m_min.x) ? false : overlap; + overlap = (triAabb.m_min.z > aabbs[bodyIndexB].m_max.z || triAabb.m_max.z < aabbs[bodyIndexB].m_min.z) ? false : overlap; + overlap = (triAabb.m_min.y > aabbs[bodyIndexB].m_max.y || triAabb.m_max.y < aabbs[bodyIndexB].m_min.y) ? false : overlap; + + if (overlap) + { + float dmin = dmins[i]; + int hasSeparatingAxis=5; + float4 sepAxis=make_float4(1,2,3,4); + sepAxis = concaveSeparatingNormalsOut[pairIdx]; + + int localCC=0; + numActualConcaveConvexTests++; + + //a triangle has 3 unique edges + convexPolyhedronA.m_numUniqueEdges = 3; + convexPolyhedronA.m_uniqueEdgesOffset = 0; + float4 uniqueEdgesA[3]; + + uniqueEdgesA[0] = (verticesA[1]-verticesA[0]); + uniqueEdgesA[1] = (verticesA[2]-verticesA[1]); + uniqueEdgesA[2] = (verticesA[0]-verticesA[2]); + + + convexPolyhedronA.m_faceOffset = 0; + + float4 normal = make_float4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); + + btGpuFace facesA[TRIANGLE_NUM_CONVEX_FACES]; + int indicesA[3+3+2+2+2]; + int curUsedIndices=0; + int fidx=0; + + //front size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[0] = 0; + indicesA[1] = 1; + indicesA[2] = 2; + curUsedIndices+=3; + float c = face.m_plane.w; + facesA[fidx].m_plane.x = normal.x; + facesA[fidx].m_plane.y = normal.y; + facesA[fidx].m_plane.z = normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + //back size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[3]=2; + indicesA[4]=1; + indicesA[5]=0; + curUsedIndices+=3; + float c = dot(normal,verticesA[0]); + float c1 = -face.m_plane.w; + facesA[fidx].m_plane.x = -normal.x; + facesA[fidx].m_plane.y = -normal.y; + facesA[fidx].m_plane.z = -normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + + bool addEdgePlanes = true; + if (addEdgePlanes) + { + int numVertices=3; + int prevVertex = numVertices-1; + for (int i=0;im_escapeIndexOrTriangleIndex&~(y));\n" +"}\n" +"int getTriangleIndexGlobal(__global const b3QuantizedBvhNode* rootNode)\n" +"{\n" +" unsigned int x=0;\n" +" unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);\n" +" // Get only the lower bits where the triangle index is stored\n" +" return (rootNode->m_escapeIndexOrTriangleIndex&~(y));\n" +"}\n" +"int isLeafNode(const b3QuantizedBvhNode* rootNode)\n" +"{\n" +" //skipindex is negative (internal node), triangleindex >=0 (leafnode)\n" +" return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;\n" +"}\n" +"int isLeafNodeGlobal(__global const b3QuantizedBvhNode* rootNode)\n" +"{\n" +" //skipindex is negative (internal node), triangleindex >=0 (leafnode)\n" +" return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;\n" +"}\n" +" \n" +"int getEscapeIndex(const b3QuantizedBvhNode* rootNode)\n" +"{\n" +" return -rootNode->m_escapeIndexOrTriangleIndex;\n" +"}\n" +"int getEscapeIndexGlobal(__global const b3QuantizedBvhNode* rootNode)\n" +"{\n" +" return -rootNode->m_escapeIndexOrTriangleIndex;\n" +"}\n" +"typedef struct\n" +"{\n" +" //12 bytes\n" +" unsigned short int m_quantizedAabbMin[3];\n" +" unsigned short int m_quantizedAabbMax[3];\n" +" //4 bytes, points to the root of the subtree\n" +" int m_rootNodeIndex;\n" +" //4 bytes\n" +" int m_subtreeSize;\n" +" int m_padding[3];\n" +"} b3BvhSubtreeInfo;\n" +"typedef struct\n" +"{\n" +" float4 m_childPosition;\n" +" float4 m_childOrientation;\n" +" int m_shapeIndex;\n" +" int m_unused0;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"} btGpuChildShape;\n" +"typedef struct\n" +"{\n" +" float4 m_pos;\n" +" float4 m_quat;\n" +" float4 m_linVel;\n" +" float4 m_angVel;\n" +" u32 m_collidableIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"} BodyData;\n" +"typedef struct \n" +"{\n" +" float4 m_localCenter;\n" +" float4 m_extents;\n" +" float4 mC;\n" +" float4 mE;\n" +" \n" +" float m_radius;\n" +" int m_faceOffset;\n" +" int m_numFaces;\n" +" int m_numVertices;\n" +" int m_vertexOffset;\n" +" int m_uniqueEdgesOffset;\n" +" int m_numUniqueEdges;\n" +" int m_unused;\n" +"} ConvexPolyhedronCL;\n" +"typedef struct \n" +"{\n" +" union\n" +" {\n" +" float4 m_min;\n" +" float m_minElems[4];\n" +" int m_minIndices[4];\n" +" };\n" +" union\n" +" {\n" +" float4 m_max;\n" +" float m_maxElems[4];\n" +" int m_maxIndices[4];\n" +" };\n" +"} btAabbCL;\n" +"#ifndef B3_AABB_H\n" +"#define B3_AABB_H\n" +"#ifndef B3_FLOAT4_H\n" +"#define B3_FLOAT4_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#define B3_PLATFORM_DEFINITIONS_H\n" +"struct MyTest\n" +"{\n" +" int bla;\n" +"};\n" +"#ifdef __cplusplus\n" +"#else\n" +"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" +"#define B3_LARGE_FLOAT 1e18f\n" +"#define B3_INFINITY 1e18f\n" +"#define b3Assert(a)\n" +"#define b3ConstArray(a) __global const a*\n" +"#define b3AtomicInc atomic_inc\n" +"#define b3AtomicAdd atomic_add\n" +"#define b3Fabs fabs\n" +"#define b3Sqrt native_sqrt\n" +"#define b3Sin native_sin\n" +"#define b3Cos native_cos\n" +"#define B3_STATIC\n" +"#endif\n" +"#endif\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Float4;\n" +" #define b3Float4ConstArg const b3Float4\n" +" #define b3MakeFloat4 (float4)\n" +" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return dot(a1, b1);\n" +" }\n" +" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return cross(a1, b1);\n" +" }\n" +" #define b3MinFloat4 min\n" +" #define b3MaxFloat4 max\n" +" #define b3Normalized(a) normalize(a)\n" +"#endif \n" +" \n" +"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" +"{\n" +" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" +" return false;\n" +" return true;\n" +"}\n" +"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" +"{\n" +" float maxDot = -B3_INFINITY;\n" +" int i = 0;\n" +" int ptIndex = -1;\n" +" for( i = 0; i < vecLen; i++ )\n" +" {\n" +" float dot = b3Dot3F4(vecArray[i],vec);\n" +" \n" +" if( dot > maxDot )\n" +" {\n" +" maxDot = dot;\n" +" ptIndex = i;\n" +" }\n" +" }\n" +" b3Assert(ptIndex>=0);\n" +" if (ptIndex<0)\n" +" {\n" +" ptIndex = 0;\n" +" }\n" +" *dotOut = maxDot;\n" +" return ptIndex;\n" +"}\n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_MAT3x3_H\n" +"#define B3_MAT3x3_H\n" +"#ifndef B3_QUAT_H\n" +"#define B3_QUAT_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif\n" +"#endif\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Quat;\n" +" #define b3QuatConstArg const b3Quat\n" +" \n" +" \n" +"inline float4 b3FastNormalize4(float4 v)\n" +"{\n" +" v = (float4)(v.xyz,0.f);\n" +" return fast_normalize(v);\n" +"}\n" +" \n" +"inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" +"inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" +"inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" +"{\n" +" b3Quat ans;\n" +" ans = b3Cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - b3Dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" +"{\n" +" b3Quat q;\n" +" q=in;\n" +" //return b3FastNormalize4(in);\n" +" float len = native_sqrt(dot(q, q));\n" +" if(len > 0.f)\n" +" {\n" +" q *= 1.f / len;\n" +" }\n" +" else\n" +" {\n" +" q.x = q.y = q.z = 0.f;\n" +" q.w = 1.f;\n" +" }\n" +" return q;\n" +"}\n" +"inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" b3Quat qInv = b3QuatInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" return b3QuatRotate( b3QuatInvert( q ), vec );\n" +"}\n" +"inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" +"{\n" +" return b3QuatRotate( orientation, point ) + (translation);\n" +"}\n" +" \n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"typedef struct\n" +"{\n" +" b3Float4 m_row[3];\n" +"}b3Mat3x3;\n" +"#define b3Mat3x3ConstArg const b3Mat3x3\n" +"#define b3GetRow(m,row) (m.m_row[row])\n" +"inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" +"{\n" +" b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" +" b3Mat3x3 out;\n" +" out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" +" out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" +" out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" +" out.m_row[0].w = 0.f;\n" +" out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" +" out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" +" out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" +" out.m_row[1].w = 0.f;\n" +" out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" +" out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" +" out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" +" out.m_row[2].w = 0.f;\n" +" return out;\n" +"}\n" +"inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = fabs(matIn.m_row[0]);\n" +" out.m_row[1] = fabs(matIn.m_row[1]);\n" +" out.m_row[2] = fabs(matIn.m_row[2]);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtZero();\n" +"__inline\n" +"b3Mat3x3 mtIdentity();\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Mat3x3 mtZero()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(0.f);\n" +" m.m_row[1] = (b3Float4)(0.f);\n" +" m.m_row[2] = (b3Float4)(0.f);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtIdentity()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(1,0,0,0);\n" +" m.m_row[1] = (b3Float4)(0,1,0,0);\n" +" m.m_row[2] = (b3Float4)(0,0,1,0);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" +" out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" +" out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" +"{\n" +" b3Mat3x3 transB;\n" +" transB = mtTranspose( b );\n" +" b3Mat3x3 ans;\n" +" // why this doesn't run when 0ing in the for{}\n" +" a.m_row[0].w = 0.f;\n" +" a.m_row[1].w = 0.f;\n" +" a.m_row[2].w = 0.f;\n" +" for(int i=0; i<3; i++)\n" +" {\n" +"// a.m_row[i].w = 0.f;\n" +" ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" +" ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" +" ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" +" ans.m_row[i].w = 0.f;\n" +" }\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" +"{\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a.m_row[0], b );\n" +" ans.y = b3Dot3F4( a.m_row[1], b );\n" +" ans.z = b3Dot3F4( a.m_row[2], b );\n" +" ans.w = 0.f;\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" +"{\n" +" b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" +" b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" +" b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a, colx );\n" +" ans.y = b3Dot3F4( a, coly );\n" +" ans.z = b3Dot3F4( a, colz );\n" +" return ans;\n" +"}\n" +"#endif\n" +"#endif //B3_MAT3x3_H\n" +"typedef struct b3Aabb b3Aabb_t;\n" +"struct b3Aabb\n" +"{\n" +" union\n" +" {\n" +" float m_min[4];\n" +" b3Float4 m_minVec;\n" +" int m_minIndices[4];\n" +" };\n" +" union\n" +" {\n" +" float m_max[4];\n" +" b3Float4 m_maxVec;\n" +" int m_signedMaxIndices[4];\n" +" };\n" +"};\n" +"inline void b3TransformAabb2(b3Float4ConstArg localAabbMin,b3Float4ConstArg localAabbMax, float margin,\n" +" b3Float4ConstArg pos,\n" +" b3QuatConstArg orn,\n" +" b3Float4* aabbMinOut,b3Float4* aabbMaxOut)\n" +"{\n" +" b3Float4 localHalfExtents = 0.5f*(localAabbMax-localAabbMin);\n" +" localHalfExtents+=b3MakeFloat4(margin,margin,margin,0.f);\n" +" b3Float4 localCenter = 0.5f*(localAabbMax+localAabbMin);\n" +" b3Mat3x3 m;\n" +" m = b3QuatGetRotationMatrix(orn);\n" +" b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);\n" +" b3Float4 center = b3TransformPoint(localCenter,pos,orn);\n" +" \n" +" b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents,b3GetRow(abs_b,0)),\n" +" b3Dot3F4(localHalfExtents,b3GetRow(abs_b,1)),\n" +" b3Dot3F4(localHalfExtents,b3GetRow(abs_b,2)),\n" +" 0.f);\n" +" *aabbMinOut = center-extent;\n" +" *aabbMaxOut = center+extent;\n" +"}\n" +"/// conservative test for overlap between two aabbs\n" +"inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1,b3Float4ConstArg aabbMax1,\n" +" b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)\n" +"{\n" +" bool overlap = true;\n" +" overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap;\n" +" overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap;\n" +" overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap;\n" +" return overlap;\n" +"}\n" +"#endif //B3_AABB_H\n" +"/*\n" +"Bullet Continuous Collision Detection and Physics Library\n" +"Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org\n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose,\n" +"including commercial applications, and to alter it and redistribute it freely,\n" +"subject to the following restrictions:\n" +"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.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"#ifndef B3_INT2_H\n" +"#define B3_INT2_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#define b3UnsignedInt2 uint2\n" +"#define b3Int2 int2\n" +"#define b3MakeInt2 (int2)\n" +"#endif //__cplusplus\n" +"#endif\n" +"typedef struct\n" +"{\n" +" float4 m_plane;\n" +" int m_indexOffset;\n" +" int m_numIndices;\n" +"} btGpuFace;\n" +"#define make_float4 (float4)\n" +"__inline\n" +"float4 cross3(float4 a, float4 b)\n" +"{\n" +" return cross(a,b);\n" +" \n" +"// float4 a1 = make_float4(a.xyz,0.f);\n" +"// float4 b1 = make_float4(b.xyz,0.f);\n" +"// return cross(a1,b1);\n" +"//float4 c = make_float4(a.y*b.z - a.z*b.y,a.z*b.x - a.x*b.z,a.x*b.y - a.y*b.x,0.f);\n" +" \n" +" // float4 c = make_float4(a.y*b.z - a.z*b.y,1.f,a.x*b.y - a.y*b.x,0.f);\n" +" \n" +" //return c;\n" +"}\n" +"__inline\n" +"float dot3F4(float4 a, float4 b)\n" +"{\n" +" float4 a1 = make_float4(a.xyz,0.f);\n" +" float4 b1 = make_float4(b.xyz,0.f);\n" +" return dot(a1, b1);\n" +"}\n" +"__inline\n" +"float4 fastNormalize4(float4 v)\n" +"{\n" +" v = make_float4(v.xyz,0.f);\n" +" return fast_normalize(v);\n" +"}\n" +"///////////////////////////////////////\n" +"// Quaternion\n" +"///////////////////////////////////////\n" +"typedef float4 Quaternion;\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b);\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in);\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec);\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q);\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b)\n" +"{\n" +" Quaternion ans;\n" +" ans = cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in)\n" +"{\n" +" return fastNormalize4(in);\n" +"// in /= length( in );\n" +"// return in;\n" +"}\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec)\n" +"{\n" +" Quaternion qInv = qtInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q)\n" +"{\n" +" return (Quaternion)(-q.xyz, q.w);\n" +"}\n" +"__inline\n" +"float4 qtInvRotate(const Quaternion q, float4 vec)\n" +"{\n" +" return qtRotate( qtInvert( q ), vec );\n" +"}\n" +"__inline\n" +"float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)\n" +"{\n" +" return qtRotate( *orientation, *p ) + (*translation);\n" +"}\n" +"__inline\n" +"float4 normalize3(const float4 a)\n" +"{\n" +" float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" +" return fastNormalize4( n );\n" +"}\n" +"inline void projectLocal(const ConvexPolyhedronCL* hull, const float4 pos, const float4 orn, \n" +"const float4* dir, const float4* vertices, float* min, float* max)\n" +"{\n" +" min[0] = FLT_MAX;\n" +" max[0] = -FLT_MAX;\n" +" int numVerts = hull->m_numVertices;\n" +" const float4 localDir = qtInvRotate(orn,*dir);\n" +" float offset = dot(pos,*dir);\n" +" for(int i=0;im_vertexOffset+i],localDir);\n" +" if(dp < min[0]) \n" +" min[0] = dp;\n" +" if(dp > max[0]) \n" +" max[0] = dp;\n" +" }\n" +" if(min[0]>max[0])\n" +" {\n" +" float tmp = min[0];\n" +" min[0] = max[0];\n" +" max[0] = tmp;\n" +" }\n" +" min[0] += offset;\n" +" max[0] += offset;\n" +"}\n" +"inline void project(__global const ConvexPolyhedronCL* hull, const float4 pos, const float4 orn, \n" +"const float4* dir, __global const float4* vertices, float* min, float* max)\n" +"{\n" +" min[0] = FLT_MAX;\n" +" max[0] = -FLT_MAX;\n" +" int numVerts = hull->m_numVertices;\n" +" const float4 localDir = qtInvRotate(orn,*dir);\n" +" float offset = dot(pos,*dir);\n" +" for(int i=0;im_vertexOffset+i],localDir);\n" +" if(dp < min[0]) \n" +" min[0] = dp;\n" +" if(dp > max[0]) \n" +" max[0] = dp;\n" +" }\n" +" if(min[0]>max[0])\n" +" {\n" +" float tmp = min[0];\n" +" min[0] = max[0];\n" +" max[0] = tmp;\n" +" }\n" +" min[0] += offset;\n" +" max[0] += offset;\n" +"}\n" +"inline bool TestSepAxisLocalA(const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, \n" +" const float4 posA,const float4 ornA,\n" +" const float4 posB,const float4 ornB,\n" +" float4* sep_axis, const float4* verticesA, __global const float4* verticesB,float* depth)\n" +"{\n" +" float Min0,Max0;\n" +" float Min1,Max1;\n" +" projectLocal(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0);\n" +" project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1);\n" +" if(Max01e-6f || fabs(v.y)>1e-6f || fabs(v.z)>1e-6f)\n" +" return false;\n" +" return true;\n" +"}\n" +"bool findSeparatingAxisLocalA( const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, \n" +" const float4 posA1,\n" +" const float4 ornA,\n" +" const float4 posB1,\n" +" const float4 ornB,\n" +" const float4 DeltaC2,\n" +" \n" +" const float4* verticesA, \n" +" const float4* uniqueEdgesA, \n" +" const btGpuFace* facesA,\n" +" const int* indicesA,\n" +" __global const float4* verticesB, \n" +" __global const float4* uniqueEdgesB, \n" +" __global const btGpuFace* facesB,\n" +" __global const int* indicesB,\n" +" float4* sep,\n" +" float* dmin)\n" +"{\n" +" \n" +" float4 posA = posA1;\n" +" posA.w = 0.f;\n" +" float4 posB = posB1;\n" +" posB.w = 0.f;\n" +" int curPlaneTests=0;\n" +" {\n" +" int numFacesA = hullA->m_numFaces;\n" +" // Test normals from hullA\n" +" for(int i=0;im_faceOffset+i].m_plane;\n" +" float4 faceANormalWS = qtRotate(ornA,normal);\n" +" if (dot3F4(DeltaC2,faceANormalWS)<0)\n" +" faceANormalWS*=-1.f;\n" +" curPlaneTests++;\n" +" float d;\n" +" if(!TestSepAxisLocalA( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d))\n" +" return false;\n" +" if(d<*dmin)\n" +" {\n" +" *dmin = d;\n" +" *sep = faceANormalWS;\n" +" }\n" +" }\n" +" }\n" +" if((dot3F4(-DeltaC2,*sep))>0.0f)\n" +" {\n" +" *sep = -(*sep);\n" +" }\n" +" return true;\n" +"}\n" +"bool findSeparatingAxisLocalB( __global const ConvexPolyhedronCL* hullA, const ConvexPolyhedronCL* hullB, \n" +" const float4 posA1,\n" +" const float4 ornA,\n" +" const float4 posB1,\n" +" const float4 ornB,\n" +" const float4 DeltaC2,\n" +" __global const float4* verticesA, \n" +" __global const float4* uniqueEdgesA, \n" +" __global const btGpuFace* facesA,\n" +" __global const int* indicesA,\n" +" const float4* verticesB,\n" +" const float4* uniqueEdgesB, \n" +" const btGpuFace* facesB,\n" +" const int* indicesB,\n" +" float4* sep,\n" +" float* dmin)\n" +"{\n" +" float4 posA = posA1;\n" +" posA.w = 0.f;\n" +" float4 posB = posB1;\n" +" posB.w = 0.f;\n" +" int curPlaneTests=0;\n" +" {\n" +" int numFacesA = hullA->m_numFaces;\n" +" // Test normals from hullA\n" +" for(int i=0;im_faceOffset+i].m_plane;\n" +" float4 faceANormalWS = qtRotate(ornA,normal);\n" +" if (dot3F4(DeltaC2,faceANormalWS)<0)\n" +" faceANormalWS *= -1.f;\n" +" curPlaneTests++;\n" +" float d;\n" +" if(!TestSepAxisLocalA( hullB, hullA, posB,ornB,posA,ornA, &faceANormalWS, verticesB,verticesA, &d))\n" +" return false;\n" +" if(d<*dmin)\n" +" {\n" +" *dmin = d;\n" +" *sep = faceANormalWS;\n" +" }\n" +" }\n" +" }\n" +" if((dot3F4(-DeltaC2,*sep))>0.0f)\n" +" {\n" +" *sep = -(*sep);\n" +" }\n" +" return true;\n" +"}\n" +"bool findSeparatingAxisEdgeEdgeLocalA( const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, \n" +" const float4 posA1,\n" +" const float4 ornA,\n" +" const float4 posB1,\n" +" const float4 ornB,\n" +" const float4 DeltaC2,\n" +" const float4* verticesA, \n" +" const float4* uniqueEdgesA, \n" +" const btGpuFace* facesA,\n" +" const int* indicesA,\n" +" __global const float4* verticesB, \n" +" __global const float4* uniqueEdgesB, \n" +" __global const btGpuFace* facesB,\n" +" __global const int* indicesB,\n" +" float4* sep,\n" +" float* dmin)\n" +"{\n" +" float4 posA = posA1;\n" +" posA.w = 0.f;\n" +" float4 posB = posB1;\n" +" posB.w = 0.f;\n" +" int curPlaneTests=0;\n" +" int curEdgeEdge = 0;\n" +" // Test edges\n" +" for(int e0=0;e0m_numUniqueEdges;e0++)\n" +" {\n" +" const float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0];\n" +" float4 edge0World = qtRotate(ornA,edge0);\n" +" for(int e1=0;e1m_numUniqueEdges;e1++)\n" +" {\n" +" const float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1];\n" +" float4 edge1World = qtRotate(ornB,edge1);\n" +" float4 crossje = cross3(edge0World,edge1World);\n" +" curEdgeEdge++;\n" +" if(!IsAlmostZero(crossje))\n" +" {\n" +" crossje = normalize3(crossje);\n" +" if (dot3F4(DeltaC2,crossje)<0)\n" +" crossje *= -1.f;\n" +" float dist;\n" +" bool result = true;\n" +" {\n" +" float Min0,Max0;\n" +" float Min1,Max1;\n" +" projectLocal(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0);\n" +" project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1);\n" +" \n" +" if(Max00.0f)\n" +" {\n" +" *sep = -(*sep);\n" +" }\n" +" return true;\n" +"}\n" +"inline int findClippingFaces(const float4 separatingNormal,\n" +" const ConvexPolyhedronCL* hullA, \n" +" __global const ConvexPolyhedronCL* hullB,\n" +" const float4 posA, const Quaternion ornA,const float4 posB, const Quaternion ornB,\n" +" __global float4* worldVertsA1,\n" +" __global float4* worldNormalsA1,\n" +" __global float4* worldVertsB1,\n" +" int capacityWorldVerts,\n" +" const float minDist, float maxDist,\n" +" const float4* verticesA,\n" +" const btGpuFace* facesA,\n" +" const int* indicesA,\n" +" __global const float4* verticesB,\n" +" __global const btGpuFace* facesB,\n" +" __global const int* indicesB,\n" +" __global int4* clippingFaces, int pairIndex)\n" +"{\n" +" int numContactsOut = 0;\n" +" int numWorldVertsB1= 0;\n" +" \n" +" \n" +" int closestFaceB=0;\n" +" float dmax = -FLT_MAX;\n" +" \n" +" {\n" +" for(int face=0;facem_numFaces;face++)\n" +" {\n" +" const float4 Normal = make_float4(facesB[hullB->m_faceOffset+face].m_plane.x,\n" +" facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f);\n" +" const float4 WorldNormal = qtRotate(ornB, Normal);\n" +" float d = dot3F4(WorldNormal,separatingNormal);\n" +" if (d > dmax)\n" +" {\n" +" dmax = d;\n" +" closestFaceB = face;\n" +" }\n" +" }\n" +" }\n" +" \n" +" {\n" +" const btGpuFace polyB = facesB[hullB->m_faceOffset+closestFaceB];\n" +" int numVertices = polyB.m_numIndices;\n" +" if (numVertices>capacityWorldVerts)\n" +" numVertices = capacityWorldVerts;\n" +" if (numVertices<0)\n" +" numVertices = 0;\n" +" \n" +" for(int e0=0;e0m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];\n" +" worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = transform(&b,&posB,&ornB);\n" +" }\n" +" }\n" +" }\n" +" \n" +" int closestFaceA=0;\n" +" {\n" +" float dmin = FLT_MAX;\n" +" for(int face=0;facem_numFaces;face++)\n" +" {\n" +" const float4 Normal = make_float4(\n" +" facesA[hullA->m_faceOffset+face].m_plane.x,\n" +" facesA[hullA->m_faceOffset+face].m_plane.y,\n" +" facesA[hullA->m_faceOffset+face].m_plane.z,\n" +" 0.f);\n" +" const float4 faceANormalWS = qtRotate(ornA,Normal);\n" +" \n" +" float d = dot3F4(faceANormalWS,separatingNormal);\n" +" if (d < dmin)\n" +" {\n" +" dmin = d;\n" +" closestFaceA = face;\n" +" worldNormalsA1[pairIndex] = faceANormalWS;\n" +" }\n" +" }\n" +" }\n" +" \n" +" int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices;\n" +" if (numVerticesA>capacityWorldVerts)\n" +" numVerticesA = capacityWorldVerts;\n" +" if (numVerticesA<0)\n" +" numVerticesA=0;\n" +" \n" +" for(int e0=0;e0m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]];\n" +" worldVertsA1[pairIndex*capacityWorldVerts+e0] = transform(&a, &posA,&ornA);\n" +" }\n" +" }\n" +" \n" +" clippingFaces[pairIndex].x = closestFaceA;\n" +" clippingFaces[pairIndex].y = closestFaceB;\n" +" clippingFaces[pairIndex].z = numVerticesA;\n" +" clippingFaces[pairIndex].w = numWorldVertsB1;\n" +" \n" +" \n" +" return numContactsOut;\n" +"}\n" +"// work-in-progress\n" +"__kernel void findConcaveSeparatingAxisVertexFaceKernel( __global int4* concavePairs,\n" +" __global const BodyData* rigidBodies,\n" +" __global const btCollidableGpu* collidables,\n" +" __global const ConvexPolyhedronCL* convexShapes,\n" +" __global const float4* vertices,\n" +" __global const float4* uniqueEdges,\n" +" __global const btGpuFace* faces,\n" +" __global const int* indices,\n" +" __global const btGpuChildShape* gpuChildShapes,\n" +" __global btAabbCL* aabbs,\n" +" __global float4* concaveSeparatingNormalsOut,\n" +" __global int* concaveHasSeparatingNormals,\n" +" __global int4* clippingFacesOut,\n" +" __global float4* worldVertsA1GPU,\n" +" __global float4* worldNormalsAGPU,\n" +" __global float4* worldVertsB1GPU,\n" +" __global float* dmins,\n" +" int vertexFaceCapacity,\n" +" int numConcavePairs\n" +" )\n" +"{\n" +" \n" +" int i = get_global_id(0);\n" +" if (i>=numConcavePairs)\n" +" return;\n" +" \n" +" concaveHasSeparatingNormals[i] = 0;\n" +" \n" +" int pairIdx = i;\n" +" \n" +" int bodyIndexA = concavePairs[i].x;\n" +" int bodyIndexB = concavePairs[i].y;\n" +" \n" +" int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" +" int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;\n" +" \n" +" int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" +" int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" +" \n" +" if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&&\n" +" collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS)\n" +" {\n" +" concavePairs[pairIdx].w = -1;\n" +" return;\n" +" }\n" +" \n" +" \n" +" \n" +" int numFacesA = convexShapes[shapeIndexA].m_numFaces;\n" +" int numActualConcaveConvexTests = 0;\n" +" \n" +" int f = concavePairs[i].z;\n" +" \n" +" bool overlap = false;\n" +" \n" +" ConvexPolyhedronCL convexPolyhedronA;\n" +" \n" +" //add 3 vertices of the triangle\n" +" convexPolyhedronA.m_numVertices = 3;\n" +" convexPolyhedronA.m_vertexOffset = 0;\n" +" float4 localCenter = make_float4(0.f,0.f,0.f,0.f);\n" +" \n" +" btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];\n" +" float4 triMinAabb, triMaxAabb;\n" +" btAabbCL triAabb;\n" +" triAabb.m_min = make_float4(1e30f,1e30f,1e30f,0.f);\n" +" triAabb.m_max = make_float4(-1e30f,-1e30f,-1e30f,0.f);\n" +" \n" +" float4 verticesA[3];\n" +" for (int i=0;i<3;i++)\n" +" {\n" +" int index = indices[face.m_indexOffset+i];\n" +" float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];\n" +" verticesA[i] = vert;\n" +" localCenter += vert;\n" +" \n" +" triAabb.m_min = min(triAabb.m_min,vert);\n" +" triAabb.m_max = max(triAabb.m_max,vert);\n" +" \n" +" }\n" +" \n" +" overlap = true;\n" +" overlap = (triAabb.m_min.x > aabbs[bodyIndexB].m_max.x || triAabb.m_max.x < aabbs[bodyIndexB].m_min.x) ? false : overlap;\n" +" overlap = (triAabb.m_min.z > aabbs[bodyIndexB].m_max.z || triAabb.m_max.z < aabbs[bodyIndexB].m_min.z) ? false : overlap;\n" +" overlap = (triAabb.m_min.y > aabbs[bodyIndexB].m_max.y || triAabb.m_max.y < aabbs[bodyIndexB].m_min.y) ? false : overlap;\n" +" \n" +" if (overlap)\n" +" {\n" +" float dmin = FLT_MAX;\n" +" int hasSeparatingAxis=5;\n" +" float4 sepAxis=make_float4(1,2,3,4);\n" +" \n" +" int localCC=0;\n" +" numActualConcaveConvexTests++;\n" +" \n" +" //a triangle has 3 unique edges\n" +" convexPolyhedronA.m_numUniqueEdges = 3;\n" +" convexPolyhedronA.m_uniqueEdgesOffset = 0;\n" +" float4 uniqueEdgesA[3];\n" +" \n" +" uniqueEdgesA[0] = (verticesA[1]-verticesA[0]);\n" +" uniqueEdgesA[1] = (verticesA[2]-verticesA[1]);\n" +" uniqueEdgesA[2] = (verticesA[0]-verticesA[2]);\n" +" \n" +" \n" +" convexPolyhedronA.m_faceOffset = 0;\n" +" \n" +" float4 normal = make_float4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);\n" +" \n" +" btGpuFace facesA[TRIANGLE_NUM_CONVEX_FACES];\n" +" int indicesA[3+3+2+2+2];\n" +" int curUsedIndices=0;\n" +" int fidx=0;\n" +" \n" +" //front size of triangle\n" +" {\n" +" facesA[fidx].m_indexOffset=curUsedIndices;\n" +" indicesA[0] = 0;\n" +" indicesA[1] = 1;\n" +" indicesA[2] = 2;\n" +" curUsedIndices+=3;\n" +" float c = face.m_plane.w;\n" +" facesA[fidx].m_plane.x = normal.x;\n" +" facesA[fidx].m_plane.y = normal.y;\n" +" facesA[fidx].m_plane.z = normal.z;\n" +" facesA[fidx].m_plane.w = c;\n" +" facesA[fidx].m_numIndices=3;\n" +" }\n" +" fidx++;\n" +" //back size of triangle\n" +" {\n" +" facesA[fidx].m_indexOffset=curUsedIndices;\n" +" indicesA[3]=2;\n" +" indicesA[4]=1;\n" +" indicesA[5]=0;\n" +" curUsedIndices+=3;\n" +" float c = dot(normal,verticesA[0]);\n" +" float c1 = -face.m_plane.w;\n" +" facesA[fidx].m_plane.x = -normal.x;\n" +" facesA[fidx].m_plane.y = -normal.y;\n" +" facesA[fidx].m_plane.z = -normal.z;\n" +" facesA[fidx].m_plane.w = c;\n" +" facesA[fidx].m_numIndices=3;\n" +" }\n" +" fidx++;\n" +" \n" +" bool addEdgePlanes = true;\n" +" if (addEdgePlanes)\n" +" {\n" +" int numVertices=3;\n" +" int prevVertex = numVertices-1;\n" +" for (int i=0;i=numConcavePairs)\n" +" return;\n" +" \n" +" if (!concaveHasSeparatingNormals[i])\n" +" return;\n" +" \n" +" int pairIdx = i;\n" +" \n" +" int bodyIndexA = concavePairs[i].x;\n" +" int bodyIndexB = concavePairs[i].y;\n" +" \n" +" int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" +" int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;\n" +" \n" +" int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" +" int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" +" \n" +" \n" +" int numFacesA = convexShapes[shapeIndexA].m_numFaces;\n" +" int numActualConcaveConvexTests = 0;\n" +" \n" +" int f = concavePairs[i].z;\n" +" \n" +" bool overlap = false;\n" +" \n" +" ConvexPolyhedronCL convexPolyhedronA;\n" +" \n" +" //add 3 vertices of the triangle\n" +" convexPolyhedronA.m_numVertices = 3;\n" +" convexPolyhedronA.m_vertexOffset = 0;\n" +" float4 localCenter = make_float4(0.f,0.f,0.f,0.f);\n" +" \n" +" btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];\n" +" float4 triMinAabb, triMaxAabb;\n" +" btAabbCL triAabb;\n" +" triAabb.m_min = make_float4(1e30f,1e30f,1e30f,0.f);\n" +" triAabb.m_max = make_float4(-1e30f,-1e30f,-1e30f,0.f);\n" +" \n" +" float4 verticesA[3];\n" +" for (int i=0;i<3;i++)\n" +" {\n" +" int index = indices[face.m_indexOffset+i];\n" +" float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];\n" +" verticesA[i] = vert;\n" +" localCenter += vert;\n" +" \n" +" triAabb.m_min = min(triAabb.m_min,vert);\n" +" triAabb.m_max = max(triAabb.m_max,vert);\n" +" \n" +" }\n" +" \n" +" overlap = true;\n" +" overlap = (triAabb.m_min.x > aabbs[bodyIndexB].m_max.x || triAabb.m_max.x < aabbs[bodyIndexB].m_min.x) ? false : overlap;\n" +" overlap = (triAabb.m_min.z > aabbs[bodyIndexB].m_max.z || triAabb.m_max.z < aabbs[bodyIndexB].m_min.z) ? false : overlap;\n" +" overlap = (triAabb.m_min.y > aabbs[bodyIndexB].m_max.y || triAabb.m_max.y < aabbs[bodyIndexB].m_min.y) ? false : overlap;\n" +" \n" +" if (overlap)\n" +" {\n" +" float dmin = dmins[i];\n" +" int hasSeparatingAxis=5;\n" +" float4 sepAxis=make_float4(1,2,3,4);\n" +" sepAxis = concaveSeparatingNormalsOut[pairIdx];\n" +" \n" +" int localCC=0;\n" +" numActualConcaveConvexTests++;\n" +" \n" +" //a triangle has 3 unique edges\n" +" convexPolyhedronA.m_numUniqueEdges = 3;\n" +" convexPolyhedronA.m_uniqueEdgesOffset = 0;\n" +" float4 uniqueEdgesA[3];\n" +" \n" +" uniqueEdgesA[0] = (verticesA[1]-verticesA[0]);\n" +" uniqueEdgesA[1] = (verticesA[2]-verticesA[1]);\n" +" uniqueEdgesA[2] = (verticesA[0]-verticesA[2]);\n" +" \n" +" \n" +" convexPolyhedronA.m_faceOffset = 0;\n" +" \n" +" float4 normal = make_float4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);\n" +" \n" +" btGpuFace facesA[TRIANGLE_NUM_CONVEX_FACES];\n" +" int indicesA[3+3+2+2+2];\n" +" int curUsedIndices=0;\n" +" int fidx=0;\n" +" \n" +" //front size of triangle\n" +" {\n" +" facesA[fidx].m_indexOffset=curUsedIndices;\n" +" indicesA[0] = 0;\n" +" indicesA[1] = 1;\n" +" indicesA[2] = 2;\n" +" curUsedIndices+=3;\n" +" float c = face.m_plane.w;\n" +" facesA[fidx].m_plane.x = normal.x;\n" +" facesA[fidx].m_plane.y = normal.y;\n" +" facesA[fidx].m_plane.z = normal.z;\n" +" facesA[fidx].m_plane.w = c;\n" +" facesA[fidx].m_numIndices=3;\n" +" }\n" +" fidx++;\n" +" //back size of triangle\n" +" {\n" +" facesA[fidx].m_indexOffset=curUsedIndices;\n" +" indicesA[3]=2;\n" +" indicesA[4]=1;\n" +" indicesA[5]=0;\n" +" curUsedIndices+=3;\n" +" float c = dot(normal,verticesA[0]);\n" +" float c1 = -face.m_plane.w;\n" +" facesA[fidx].m_plane.x = -normal.x;\n" +" facesA[fidx].m_plane.y = -normal.y;\n" +" facesA[fidx].m_plane.z = -normal.z;\n" +" facesA[fidx].m_plane.w = c;\n" +" facesA[fidx].m_numIndices=3;\n" +" }\n" +" fidx++;\n" +" \n" +" bool addEdgePlanes = true;\n" +" if (addEdgePlanes)\n" +" {\n" +" int numVertices=3;\n" +" int prevVertex = numVertices-1;\n" +" for (int i=0;im_escapeIndexOrTriangleIndex&~(y));\n" +"}\n" +"int getTriangleIndexGlobal(__global const b3QuantizedBvhNode* rootNode)\n" +"{\n" +" unsigned int x=0;\n" +" unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);\n" +" // Get only the lower bits where the triangle index is stored\n" +" return (rootNode->m_escapeIndexOrTriangleIndex&~(y));\n" +"}\n" +"int isLeafNode(const b3QuantizedBvhNode* rootNode)\n" +"{\n" +" //skipindex is negative (internal node), triangleindex >=0 (leafnode)\n" +" return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;\n" +"}\n" +"int isLeafNodeGlobal(__global const b3QuantizedBvhNode* rootNode)\n" +"{\n" +" //skipindex is negative (internal node), triangleindex >=0 (leafnode)\n" +" return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;\n" +"}\n" +" \n" +"int getEscapeIndex(const b3QuantizedBvhNode* rootNode)\n" +"{\n" +" return -rootNode->m_escapeIndexOrTriangleIndex;\n" +"}\n" +"int getEscapeIndexGlobal(__global const b3QuantizedBvhNode* rootNode)\n" +"{\n" +" return -rootNode->m_escapeIndexOrTriangleIndex;\n" +"}\n" +"typedef struct\n" +"{\n" +" //12 bytes\n" +" unsigned short int m_quantizedAabbMin[3];\n" +" unsigned short int m_quantizedAabbMax[3];\n" +" //4 bytes, points to the root of the subtree\n" +" int m_rootNodeIndex;\n" +" //4 bytes\n" +" int m_subtreeSize;\n" +" int m_padding[3];\n" +"} b3BvhSubtreeInfo;\n" +"typedef struct\n" +"{\n" +" float4 m_childPosition;\n" +" float4 m_childOrientation;\n" +" int m_shapeIndex;\n" +" int m_unused0;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"} btGpuChildShape;\n" +"typedef struct\n" +"{\n" +" float4 m_pos;\n" +" float4 m_quat;\n" +" float4 m_linVel;\n" +" float4 m_angVel;\n" +" u32 m_collidableIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"} BodyData;\n" +"typedef struct \n" +"{\n" +" float4 m_localCenter;\n" +" float4 m_extents;\n" +" float4 mC;\n" +" float4 mE;\n" +" \n" +" float m_radius;\n" +" int m_faceOffset;\n" +" int m_numFaces;\n" +" int m_numVertices;\n" +" int m_vertexOffset;\n" +" int m_uniqueEdgesOffset;\n" +" int m_numUniqueEdges;\n" +" int m_unused;\n" +"} ConvexPolyhedronCL;\n" +"typedef struct \n" +"{\n" +" union\n" +" {\n" +" float4 m_min;\n" +" float m_minElems[4];\n" +" int m_minIndices[4];\n" +" };\n" +" union\n" +" {\n" +" float4 m_max;\n" +" float m_maxElems[4];\n" +" int m_maxIndices[4];\n" +" };\n" +"} btAabbCL;\n" +"#ifndef B3_AABB_H\n" +"#define B3_AABB_H\n" +"#ifndef B3_FLOAT4_H\n" +"#define B3_FLOAT4_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#define B3_PLATFORM_DEFINITIONS_H\n" +"struct MyTest\n" +"{\n" +" int bla;\n" +"};\n" +"#ifdef __cplusplus\n" +"#else\n" +"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" +"#define B3_LARGE_FLOAT 1e18f\n" +"#define B3_INFINITY 1e18f\n" +"#define b3Assert(a)\n" +"#define b3ConstArray(a) __global const a*\n" +"#define b3AtomicInc atomic_inc\n" +"#define b3AtomicAdd atomic_add\n" +"#define b3Fabs fabs\n" +"#define b3Sqrt native_sqrt\n" +"#define b3Sin native_sin\n" +"#define b3Cos native_cos\n" +"#define B3_STATIC\n" +"#endif\n" +"#endif\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Float4;\n" +" #define b3Float4ConstArg const b3Float4\n" +" #define b3MakeFloat4 (float4)\n" +" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return dot(a1, b1);\n" +" }\n" +" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return cross(a1, b1);\n" +" }\n" +" #define b3MinFloat4 min\n" +" #define b3MaxFloat4 max\n" +" #define b3Normalized(a) normalize(a)\n" +"#endif \n" +" \n" +"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" +"{\n" +" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" +" return false;\n" +" return true;\n" +"}\n" +"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" +"{\n" +" float maxDot = -B3_INFINITY;\n" +" int i = 0;\n" +" int ptIndex = -1;\n" +" for( i = 0; i < vecLen; i++ )\n" +" {\n" +" float dot = b3Dot3F4(vecArray[i],vec);\n" +" \n" +" if( dot > maxDot )\n" +" {\n" +" maxDot = dot;\n" +" ptIndex = i;\n" +" }\n" +" }\n" +" b3Assert(ptIndex>=0);\n" +" if (ptIndex<0)\n" +" {\n" +" ptIndex = 0;\n" +" }\n" +" *dotOut = maxDot;\n" +" return ptIndex;\n" +"}\n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_MAT3x3_H\n" +"#define B3_MAT3x3_H\n" +"#ifndef B3_QUAT_H\n" +"#define B3_QUAT_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif\n" +"#endif\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Quat;\n" +" #define b3QuatConstArg const b3Quat\n" +" \n" +" \n" +"inline float4 b3FastNormalize4(float4 v)\n" +"{\n" +" v = (float4)(v.xyz,0.f);\n" +" return fast_normalize(v);\n" +"}\n" +" \n" +"inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" +"inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" +"inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" +"{\n" +" b3Quat ans;\n" +" ans = b3Cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - b3Dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" +"{\n" +" b3Quat q;\n" +" q=in;\n" +" //return b3FastNormalize4(in);\n" +" float len = native_sqrt(dot(q, q));\n" +" if(len > 0.f)\n" +" {\n" +" q *= 1.f / len;\n" +" }\n" +" else\n" +" {\n" +" q.x = q.y = q.z = 0.f;\n" +" q.w = 1.f;\n" +" }\n" +" return q;\n" +"}\n" +"inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" b3Quat qInv = b3QuatInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" return b3QuatRotate( b3QuatInvert( q ), vec );\n" +"}\n" +"inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" +"{\n" +" return b3QuatRotate( orientation, point ) + (translation);\n" +"}\n" +" \n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"typedef struct\n" +"{\n" +" b3Float4 m_row[3];\n" +"}b3Mat3x3;\n" +"#define b3Mat3x3ConstArg const b3Mat3x3\n" +"#define b3GetRow(m,row) (m.m_row[row])\n" +"inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" +"{\n" +" b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" +" b3Mat3x3 out;\n" +" out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" +" out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" +" out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" +" out.m_row[0].w = 0.f;\n" +" out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" +" out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" +" out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" +" out.m_row[1].w = 0.f;\n" +" out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" +" out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" +" out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" +" out.m_row[2].w = 0.f;\n" +" return out;\n" +"}\n" +"inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = fabs(matIn.m_row[0]);\n" +" out.m_row[1] = fabs(matIn.m_row[1]);\n" +" out.m_row[2] = fabs(matIn.m_row[2]);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtZero();\n" +"__inline\n" +"b3Mat3x3 mtIdentity();\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Mat3x3 mtZero()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(0.f);\n" +" m.m_row[1] = (b3Float4)(0.f);\n" +" m.m_row[2] = (b3Float4)(0.f);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtIdentity()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(1,0,0,0);\n" +" m.m_row[1] = (b3Float4)(0,1,0,0);\n" +" m.m_row[2] = (b3Float4)(0,0,1,0);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" +" out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" +" out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" +"{\n" +" b3Mat3x3 transB;\n" +" transB = mtTranspose( b );\n" +" b3Mat3x3 ans;\n" +" // why this doesn't run when 0ing in the for{}\n" +" a.m_row[0].w = 0.f;\n" +" a.m_row[1].w = 0.f;\n" +" a.m_row[2].w = 0.f;\n" +" for(int i=0; i<3; i++)\n" +" {\n" +"// a.m_row[i].w = 0.f;\n" +" ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" +" ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" +" ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" +" ans.m_row[i].w = 0.f;\n" +" }\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" +"{\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a.m_row[0], b );\n" +" ans.y = b3Dot3F4( a.m_row[1], b );\n" +" ans.z = b3Dot3F4( a.m_row[2], b );\n" +" ans.w = 0.f;\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" +"{\n" +" b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" +" b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" +" b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a, colx );\n" +" ans.y = b3Dot3F4( a, coly );\n" +" ans.z = b3Dot3F4( a, colz );\n" +" return ans;\n" +"}\n" +"#endif\n" +"#endif //B3_MAT3x3_H\n" +"typedef struct b3Aabb b3Aabb_t;\n" +"struct b3Aabb\n" +"{\n" +" union\n" +" {\n" +" float m_min[4];\n" +" b3Float4 m_minVec;\n" +" int m_minIndices[4];\n" +" };\n" +" union\n" +" {\n" +" float m_max[4];\n" +" b3Float4 m_maxVec;\n" +" int m_signedMaxIndices[4];\n" +" };\n" +"};\n" +"inline void b3TransformAabb2(b3Float4ConstArg localAabbMin,b3Float4ConstArg localAabbMax, float margin,\n" +" b3Float4ConstArg pos,\n" +" b3QuatConstArg orn,\n" +" b3Float4* aabbMinOut,b3Float4* aabbMaxOut)\n" +"{\n" +" b3Float4 localHalfExtents = 0.5f*(localAabbMax-localAabbMin);\n" +" localHalfExtents+=b3MakeFloat4(margin,margin,margin,0.f);\n" +" b3Float4 localCenter = 0.5f*(localAabbMax+localAabbMin);\n" +" b3Mat3x3 m;\n" +" m = b3QuatGetRotationMatrix(orn);\n" +" b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);\n" +" b3Float4 center = b3TransformPoint(localCenter,pos,orn);\n" +" \n" +" b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents,b3GetRow(abs_b,0)),\n" +" b3Dot3F4(localHalfExtents,b3GetRow(abs_b,1)),\n" +" b3Dot3F4(localHalfExtents,b3GetRow(abs_b,2)),\n" +" 0.f);\n" +" *aabbMinOut = center-extent;\n" +" *aabbMaxOut = center+extent;\n" +"}\n" +"/// conservative test for overlap between two aabbs\n" +"inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1,b3Float4ConstArg aabbMax1,\n" +" b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)\n" +"{\n" +" bool overlap = true;\n" +" overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap;\n" +" overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap;\n" +" overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap;\n" +" return overlap;\n" +"}\n" +"#endif //B3_AABB_H\n" +"/*\n" +"Bullet Continuous Collision Detection and Physics Library\n" +"Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org\n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose,\n" +"including commercial applications, and to alter it and redistribute it freely,\n" +"subject to the following restrictions:\n" +"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.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"#ifndef B3_INT2_H\n" +"#define B3_INT2_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#define b3UnsignedInt2 uint2\n" +"#define b3Int2 int2\n" +"#define b3MakeInt2 (int2)\n" +"#endif //__cplusplus\n" +"#endif\n" +"typedef struct\n" +"{\n" +" float4 m_plane;\n" +" int m_indexOffset;\n" +" int m_numIndices;\n" +"} btGpuFace;\n" +"#define make_float4 (float4)\n" +"__inline\n" +"float4 cross3(float4 a, float4 b)\n" +"{\n" +" return cross(a,b);\n" +" \n" +"// float4 a1 = make_float4(a.xyz,0.f);\n" +"// float4 b1 = make_float4(b.xyz,0.f);\n" +"// return cross(a1,b1);\n" +"//float4 c = make_float4(a.y*b.z - a.z*b.y,a.z*b.x - a.x*b.z,a.x*b.y - a.y*b.x,0.f);\n" +" \n" +" // float4 c = make_float4(a.y*b.z - a.z*b.y,1.f,a.x*b.y - a.y*b.x,0.f);\n" +" \n" +" //return c;\n" +"}\n" +"__inline\n" +"float dot3F4(float4 a, float4 b)\n" +"{\n" +" float4 a1 = make_float4(a.xyz,0.f);\n" +" float4 b1 = make_float4(b.xyz,0.f);\n" +" return dot(a1, b1);\n" +"}\n" +"__inline\n" +"float4 fastNormalize4(float4 v)\n" +"{\n" +" v = make_float4(v.xyz,0.f);\n" +" return fast_normalize(v);\n" +"}\n" +"///////////////////////////////////////\n" +"// Quaternion\n" +"///////////////////////////////////////\n" +"typedef float4 Quaternion;\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b);\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in);\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec);\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q);\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b)\n" +"{\n" +" Quaternion ans;\n" +" ans = cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in)\n" +"{\n" +" return fastNormalize4(in);\n" +"// in /= length( in );\n" +"// return in;\n" +"}\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec)\n" +"{\n" +" Quaternion qInv = qtInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q)\n" +"{\n" +" return (Quaternion)(-q.xyz, q.w);\n" +"}\n" +"__inline\n" +"float4 qtInvRotate(const Quaternion q, float4 vec)\n" +"{\n" +" return qtRotate( qtInvert( q ), vec );\n" +"}\n" +"__inline\n" +"float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)\n" +"{\n" +" return qtRotate( *orientation, *p ) + (*translation);\n" +"}\n" +"__inline\n" +"float4 normalize3(const float4 a)\n" +"{\n" +" float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" +" return fastNormalize4( n );\n" +"}\n" +"inline void projectLocal(const ConvexPolyhedronCL* hull, const float4 pos, const float4 orn, \n" +"const float4* dir, const float4* vertices, float* min, float* max)\n" +"{\n" +" min[0] = FLT_MAX;\n" +" max[0] = -FLT_MAX;\n" +" int numVerts = hull->m_numVertices;\n" +" const float4 localDir = qtInvRotate(orn,*dir);\n" +" float offset = dot(pos,*dir);\n" +" for(int i=0;im_vertexOffset+i],localDir);\n" +" if(dp < min[0]) \n" +" min[0] = dp;\n" +" if(dp > max[0]) \n" +" max[0] = dp;\n" +" }\n" +" if(min[0]>max[0])\n" +" {\n" +" float tmp = min[0];\n" +" min[0] = max[0];\n" +" max[0] = tmp;\n" +" }\n" +" min[0] += offset;\n" +" max[0] += offset;\n" +"}\n" +"inline void project(__global const ConvexPolyhedronCL* hull, const float4 pos, const float4 orn, \n" +"const float4* dir, __global const float4* vertices, float* min, float* max)\n" +"{\n" +" min[0] = FLT_MAX;\n" +" max[0] = -FLT_MAX;\n" +" int numVerts = hull->m_numVertices;\n" +" const float4 localDir = qtInvRotate(orn,*dir);\n" +" float offset = dot(pos,*dir);\n" +" for(int i=0;im_vertexOffset+i],localDir);\n" +" if(dp < min[0]) \n" +" min[0] = dp;\n" +" if(dp > max[0]) \n" +" max[0] = dp;\n" +" }\n" +" if(min[0]>max[0])\n" +" {\n" +" float tmp = min[0];\n" +" min[0] = max[0];\n" +" max[0] = tmp;\n" +" }\n" +" min[0] += offset;\n" +" max[0] += offset;\n" +"}\n" +"inline bool TestSepAxisLocalA(const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, \n" +" const float4 posA,const float4 ornA,\n" +" const float4 posB,const float4 ornB,\n" +" float4* sep_axis, const float4* verticesA, __global const float4* verticesB,float* depth)\n" +"{\n" +" float Min0,Max0;\n" +" float Min1,Max1;\n" +" projectLocal(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0);\n" +" project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1);\n" +" if(Max01e-6f || fabs(v.y)>1e-6f || fabs(v.z)>1e-6f)\n" +" return false;\n" +" return true;\n" +"}\n" +"bool findSeparatingAxisLocalA( const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, \n" +" const float4 posA1,\n" +" const float4 ornA,\n" +" const float4 posB1,\n" +" const float4 ornB,\n" +" const float4 DeltaC2,\n" +" \n" +" const float4* verticesA, \n" +" const float4* uniqueEdgesA, \n" +" const btGpuFace* facesA,\n" +" const int* indicesA,\n" +" __global const float4* verticesB, \n" +" __global const float4* uniqueEdgesB, \n" +" __global const btGpuFace* facesB,\n" +" __global const int* indicesB,\n" +" float4* sep,\n" +" float* dmin)\n" +"{\n" +" \n" +" float4 posA = posA1;\n" +" posA.w = 0.f;\n" +" float4 posB = posB1;\n" +" posB.w = 0.f;\n" +" int curPlaneTests=0;\n" +" {\n" +" int numFacesA = hullA->m_numFaces;\n" +" // Test normals from hullA\n" +" for(int i=0;im_faceOffset+i].m_plane;\n" +" float4 faceANormalWS = qtRotate(ornA,normal);\n" +" if (dot3F4(DeltaC2,faceANormalWS)<0)\n" +" faceANormalWS*=-1.f;\n" +" curPlaneTests++;\n" +" float d;\n" +" if(!TestSepAxisLocalA( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d))\n" +" return false;\n" +" if(d<*dmin)\n" +" {\n" +" *dmin = d;\n" +" *sep = faceANormalWS;\n" +" }\n" +" }\n" +" }\n" +" if((dot3F4(-DeltaC2,*sep))>0.0f)\n" +" {\n" +" *sep = -(*sep);\n" +" }\n" +" return true;\n" +"}\n" +"bool findSeparatingAxisLocalB( __global const ConvexPolyhedronCL* hullA, const ConvexPolyhedronCL* hullB, \n" +" const float4 posA1,\n" +" const float4 ornA,\n" +" const float4 posB1,\n" +" const float4 ornB,\n" +" const float4 DeltaC2,\n" +" __global const float4* verticesA, \n" +" __global const float4* uniqueEdgesA, \n" +" __global const btGpuFace* facesA,\n" +" __global const int* indicesA,\n" +" const float4* verticesB,\n" +" const float4* uniqueEdgesB, \n" +" const btGpuFace* facesB,\n" +" const int* indicesB,\n" +" float4* sep,\n" +" float* dmin)\n" +"{\n" +" float4 posA = posA1;\n" +" posA.w = 0.f;\n" +" float4 posB = posB1;\n" +" posB.w = 0.f;\n" +" int curPlaneTests=0;\n" +" {\n" +" int numFacesA = hullA->m_numFaces;\n" +" // Test normals from hullA\n" +" for(int i=0;im_faceOffset+i].m_plane;\n" +" float4 faceANormalWS = qtRotate(ornA,normal);\n" +" if (dot3F4(DeltaC2,faceANormalWS)<0)\n" +" faceANormalWS *= -1.f;\n" +" curPlaneTests++;\n" +" float d;\n" +" if(!TestSepAxisLocalA( hullB, hullA, posB,ornB,posA,ornA, &faceANormalWS, verticesB,verticesA, &d))\n" +" return false;\n" +" if(d<*dmin)\n" +" {\n" +" *dmin = d;\n" +" *sep = faceANormalWS;\n" +" }\n" +" }\n" +" }\n" +" if((dot3F4(-DeltaC2,*sep))>0.0f)\n" +" {\n" +" *sep = -(*sep);\n" +" }\n" +" return true;\n" +"}\n" +"bool findSeparatingAxisEdgeEdgeLocalA( const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, \n" +" const float4 posA1,\n" +" const float4 ornA,\n" +" const float4 posB1,\n" +" const float4 ornB,\n" +" const float4 DeltaC2,\n" +" const float4* verticesA, \n" +" const float4* uniqueEdgesA, \n" +" const btGpuFace* facesA,\n" +" const int* indicesA,\n" +" __global const float4* verticesB, \n" +" __global const float4* uniqueEdgesB, \n" +" __global const btGpuFace* facesB,\n" +" __global const int* indicesB,\n" +" float4* sep,\n" +" float* dmin)\n" +"{\n" +" float4 posA = posA1;\n" +" posA.w = 0.f;\n" +" float4 posB = posB1;\n" +" posB.w = 0.f;\n" +" int curPlaneTests=0;\n" +" int curEdgeEdge = 0;\n" +" // Test edges\n" +" for(int e0=0;e0m_numUniqueEdges;e0++)\n" +" {\n" +" const float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0];\n" +" float4 edge0World = qtRotate(ornA,edge0);\n" +" for(int e1=0;e1m_numUniqueEdges;e1++)\n" +" {\n" +" const float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1];\n" +" float4 edge1World = qtRotate(ornB,edge1);\n" +" float4 crossje = cross3(edge0World,edge1World);\n" +" curEdgeEdge++;\n" +" if(!IsAlmostZero(crossje))\n" +" {\n" +" crossje = normalize3(crossje);\n" +" if (dot3F4(DeltaC2,crossje)<0)\n" +" crossje *= -1.f;\n" +" float dist;\n" +" bool result = true;\n" +" {\n" +" float Min0,Max0;\n" +" float Min1,Max1;\n" +" projectLocal(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0);\n" +" project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1);\n" +" \n" +" if(Max00.0f)\n" +" {\n" +" *sep = -(*sep);\n" +" }\n" +" return true;\n" +"}\n" +"inline bool TestSepAxis(__global const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, \n" +" const float4 posA,const float4 ornA,\n" +" const float4 posB,const float4 ornB,\n" +" float4* sep_axis, __global const float4* vertices,float* depth)\n" +"{\n" +" float Min0,Max0;\n" +" float Min1,Max1;\n" +" project(hullA,posA,ornA,sep_axis,vertices, &Min0, &Max0);\n" +" project(hullB,posB,ornB, sep_axis,vertices, &Min1, &Max1);\n" +" if(Max0m_numFaces;\n" +" // Test normals from hullA\n" +" for(int i=0;im_faceOffset+i].m_plane;\n" +" float4 faceANormalWS = qtRotate(ornA,normal);\n" +" \n" +" if (dot3F4(DeltaC2,faceANormalWS)<0)\n" +" faceANormalWS*=-1.f;\n" +" \n" +" curPlaneTests++;\n" +" \n" +" float d;\n" +" if(!TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, vertices,&d))\n" +" return false;\n" +" \n" +" if(d<*dmin)\n" +" {\n" +" *dmin = d;\n" +" *sep = faceANormalWS;\n" +" }\n" +" }\n" +" }\n" +" if((dot3F4(-DeltaC2,*sep))>0.0f)\n" +" {\n" +" *sep = -(*sep);\n" +" }\n" +" \n" +" return true;\n" +"}\n" +"bool findSeparatingAxisUnitSphere( __global const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, \n" +" const float4 posA1,\n" +" const float4 ornA,\n" +" const float4 posB1,\n" +" const float4 ornB,\n" +" const float4 DeltaC2,\n" +" __global const float4* vertices,\n" +" __global const float4* unitSphereDirections,\n" +" int numUnitSphereDirections,\n" +" float4* sep,\n" +" float* dmin)\n" +"{\n" +" \n" +" float4 posA = posA1;\n" +" posA.w = 0.f;\n" +" float4 posB = posB1;\n" +" posB.w = 0.f;\n" +" int curPlaneTests=0;\n" +" int curEdgeEdge = 0;\n" +" // Test unit sphere directions\n" +" for (int i=0;i0)\n" +" crossje *= -1.f;\n" +" {\n" +" float dist;\n" +" bool result = true;\n" +" float Min0,Max0;\n" +" float Min1,Max1;\n" +" project(hullA,posA,ornA,&crossje,vertices, &Min0, &Max0);\n" +" project(hullB,posB,ornB,&crossje,vertices, &Min1, &Max1);\n" +" \n" +" if(Max00.0f)\n" +" {\n" +" *sep = -(*sep);\n" +" }\n" +" return true;\n" +"}\n" +"bool findSeparatingAxisEdgeEdge( __global const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, \n" +" const float4 posA1,\n" +" const float4 ornA,\n" +" const float4 posB1,\n" +" const float4 ornB,\n" +" const float4 DeltaC2,\n" +" __global const float4* vertices, \n" +" __global const float4* uniqueEdges, \n" +" __global const btGpuFace* faces,\n" +" __global const int* indices,\n" +" float4* sep,\n" +" float* dmin)\n" +"{\n" +" \n" +" float4 posA = posA1;\n" +" posA.w = 0.f;\n" +" float4 posB = posB1;\n" +" posB.w = 0.f;\n" +" int curPlaneTests=0;\n" +" int curEdgeEdge = 0;\n" +" // Test edges\n" +" for(int e0=0;e0m_numUniqueEdges;e0++)\n" +" {\n" +" const float4 edge0 = uniqueEdges[hullA->m_uniqueEdgesOffset+e0];\n" +" float4 edge0World = qtRotate(ornA,edge0);\n" +" for(int e1=0;e1m_numUniqueEdges;e1++)\n" +" {\n" +" const float4 edge1 = uniqueEdges[hullB->m_uniqueEdgesOffset+e1];\n" +" float4 edge1World = qtRotate(ornB,edge1);\n" +" float4 crossje = cross3(edge0World,edge1World);\n" +" curEdgeEdge++;\n" +" if(!IsAlmostZero(crossje))\n" +" {\n" +" crossje = normalize3(crossje);\n" +" if (dot3F4(DeltaC2,crossje)<0)\n" +" crossje*=-1.f;\n" +" \n" +" float dist;\n" +" bool result = true;\n" +" {\n" +" float Min0,Max0;\n" +" float Min1,Max1;\n" +" project(hullA,posA,ornA,&crossje,vertices, &Min0, &Max0);\n" +" project(hullB,posB,ornB,&crossje,vertices, &Min1, &Max1);\n" +" \n" +" if(Max00.0f)\n" +" {\n" +" *sep = -(*sep);\n" +" }\n" +" return true;\n" +"}\n" +"// work-in-progress\n" +"__kernel void processCompoundPairsKernel( __global const int4* gpuCompoundPairs,\n" +" __global const BodyData* rigidBodies, \n" +" __global const btCollidableGpu* collidables,\n" +" __global const ConvexPolyhedronCL* convexShapes, \n" +" __global const float4* vertices,\n" +" __global const float4* uniqueEdges,\n" +" __global const btGpuFace* faces,\n" +" __global const int* indices,\n" +" __global btAabbCL* aabbs,\n" +" __global const btGpuChildShape* gpuChildShapes,\n" +" __global volatile float4* gpuCompoundSepNormalsOut,\n" +" __global volatile int* gpuHasCompoundSepNormalsOut,\n" +" int numCompoundPairs\n" +" )\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i= 0)\n" +" {\n" +" collidableIndexA = gpuChildShapes[childShapeIndexA].m_shapeIndex;\n" +" float4 childPosA = gpuChildShapes[childShapeIndexA].m_childPosition;\n" +" float4 childOrnA = gpuChildShapes[childShapeIndexA].m_childOrientation;\n" +" float4 newPosA = qtRotate(ornA,childPosA)+posA;\n" +" float4 newOrnA = qtMul(ornA,childOrnA);\n" +" posA = newPosA;\n" +" ornA = newOrnA;\n" +" } else\n" +" {\n" +" collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" +" }\n" +" \n" +" if (childShapeIndexB>=0)\n" +" {\n" +" collidableIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;\n" +" float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;\n" +" float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;\n" +" float4 newPosB = transform(&childPosB,&posB,&ornB);\n" +" float4 newOrnB = qtMul(ornB,childOrnB);\n" +" posB = newPosB;\n" +" ornB = newOrnB;\n" +" } else\n" +" {\n" +" collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; \n" +" }\n" +" \n" +" gpuHasCompoundSepNormalsOut[i] = 0;\n" +" \n" +" int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" +" int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" +" \n" +" int shapeTypeA = collidables[collidableIndexA].m_shapeType;\n" +" int shapeTypeB = collidables[collidableIndexB].m_shapeType;\n" +" \n" +" if ((shapeTypeA != SHAPE_CONVEX_HULL) || (shapeTypeB != SHAPE_CONVEX_HULL))\n" +" {\n" +" return;\n" +" }\n" +" int hasSeparatingAxis = 5;\n" +" \n" +" int numFacesA = convexShapes[shapeIndexA].m_numFaces;\n" +" float dmin = FLT_MAX;\n" +" posA.w = 0.f;\n" +" posB.w = 0.f;\n" +" float4 c0local = convexShapes[shapeIndexA].m_localCenter;\n" +" float4 c0 = transform(&c0local, &posA, &ornA);\n" +" float4 c1local = convexShapes[shapeIndexB].m_localCenter;\n" +" float4 c1 = transform(&c1local,&posB,&ornB);\n" +" const float4 DeltaC2 = c0 - c1;\n" +" float4 sepNormal = make_float4(1,0,0,0);\n" +" bool sepA = findSeparatingAxis( &convexShapes[shapeIndexA], &convexShapes[shapeIndexB],posA,ornA,posB,ornB,DeltaC2,vertices,uniqueEdges,faces,indices,&sepNormal,&dmin);\n" +" hasSeparatingAxis = 4;\n" +" if (!sepA)\n" +" {\n" +" hasSeparatingAxis = 0;\n" +" } else\n" +" {\n" +" bool sepB = findSeparatingAxis( &convexShapes[shapeIndexB],&convexShapes[shapeIndexA],posB,ornB,posA,ornA,DeltaC2,vertices,uniqueEdges,faces,indices,&sepNormal,&dmin);\n" +" if (!sepB)\n" +" {\n" +" hasSeparatingAxis = 0;\n" +" } else//(!sepB)\n" +" {\n" +" bool sepEE = findSeparatingAxisEdgeEdge( &convexShapes[shapeIndexA], &convexShapes[shapeIndexB],posA,ornA,posB,ornB,DeltaC2,vertices,uniqueEdges,faces,indices,&sepNormal,&dmin);\n" +" if (sepEE)\n" +" {\n" +" gpuCompoundSepNormalsOut[i] = sepNormal;//fastNormalize4(sepNormal);\n" +" gpuHasCompoundSepNormalsOut[i] = 1;\n" +" }//sepEE\n" +" }//(!sepB)\n" +" }//(!sepA)\n" +" \n" +" \n" +" }\n" +" \n" +"}\n" +"inline b3Float4 MyUnQuantize(const unsigned short* vecIn, b3Float4 quantization, b3Float4 bvhAabbMin)\n" +"{\n" +" b3Float4 vecOut;\n" +" vecOut = b3MakeFloat4(\n" +" (float)(vecIn[0]) / (quantization.x),\n" +" (float)(vecIn[1]) / (quantization.y),\n" +" (float)(vecIn[2]) / (quantization.z),\n" +" 0.f);\n" +" vecOut += bvhAabbMin;\n" +" return vecOut;\n" +"}\n" +"inline b3Float4 MyUnQuantizeGlobal(__global const unsigned short* vecIn, b3Float4 quantization, b3Float4 bvhAabbMin)\n" +"{\n" +" b3Float4 vecOut;\n" +" vecOut = b3MakeFloat4(\n" +" (float)(vecIn[0]) / (quantization.x),\n" +" (float)(vecIn[1]) / (quantization.y),\n" +" (float)(vecIn[2]) / (quantization.z),\n" +" 0.f);\n" +" vecOut += bvhAabbMin;\n" +" return vecOut;\n" +"}\n" +"// work-in-progress\n" +"__kernel void findCompoundPairsKernel( __global const int4* pairs, \n" +" __global const BodyData* rigidBodies, \n" +" __global const btCollidableGpu* collidables,\n" +" __global const ConvexPolyhedronCL* convexShapes, \n" +" __global const float4* vertices,\n" +" __global const float4* uniqueEdges,\n" +" __global const btGpuFace* faces,\n" +" __global const int* indices,\n" +" __global b3Aabb_t* aabbLocalSpace,\n" +" __global const btGpuChildShape* gpuChildShapes,\n" +" __global volatile int4* gpuCompoundPairsOut,\n" +" __global volatile int* numCompoundPairsOut,\n" +" __global const b3BvhSubtreeInfo* subtrees,\n" +" __global const b3QuantizedBvhNode* quantizedNodes,\n" +" __global const b3BvhInfo* bvhInfos,\n" +" int numPairs,\n" +" int maxNumCompoundPairsCapacity\n" +" )\n" +"{\n" +" int i = get_global_id(0);\n" +" if (imaxStackDepth && !(isLeafA && isLeafB))\n" +" {\n" +" //printf(\"Error: traversal exceeded maxStackDepth\");\n" +" continue;\n" +" }\n" +" if(isInternalA)\n" +" {\n" +" int nodeAleftChild = node.x+1;\n" +" bool isNodeALeftChildLeaf = isLeafNodeGlobal(&quantizedNodes[node.x+1]);\n" +" int nodeArightChild = isNodeALeftChildLeaf? node.x+2 : node.x+1 + getEscapeIndexGlobal(&quantizedNodes[node.x+1]);\n" +" if(isInternalB)\n" +" { \n" +" int nodeBleftChild = node.y+1;\n" +" bool isNodeBLeftChildLeaf = isLeafNodeGlobal(&quantizedNodes[node.y+1]);\n" +" int nodeBrightChild = isNodeBLeftChildLeaf? node.y+2 : node.y+1 + getEscapeIndexGlobal(&quantizedNodes[node.y+1]);\n" +" nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBleftChild);\n" +" nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBleftChild);\n" +" nodeStack[depth++] = b3MakeInt2(nodeAleftChild, nodeBrightChild);\n" +" nodeStack[depth++] = b3MakeInt2(nodeArightChild, nodeBrightChild);\n" +" }\n" +" else\n" +" {\n" +" nodeStack[depth++] = b3MakeInt2(nodeAleftChild,node.y);\n" +" nodeStack[depth++] = b3MakeInt2(nodeArightChild,node.y);\n" +" }\n" +" }\n" +" else\n" +" {\n" +" if(isInternalB)\n" +" {\n" +" int nodeBleftChild = node.y+1;\n" +" bool isNodeBLeftChildLeaf = isLeafNodeGlobal(&quantizedNodes[node.y+1]);\n" +" int nodeBrightChild = isNodeBLeftChildLeaf? node.y+2 : node.y+1 + getEscapeIndexGlobal(&quantizedNodes[node.y+1]);\n" +" nodeStack[depth++] = b3MakeInt2(node.x,nodeBleftChild);\n" +" nodeStack[depth++] = b3MakeInt2(node.x,nodeBrightChild);\n" +" }\n" +" else\n" +" {\n" +" int compoundPairIdx = atomic_inc(numCompoundPairsOut);\n" +" if (compoundPairIdxm_numFaces;face++)\n" +" {\n" +" const float4 Normal = make_float4(facesB[hullB->m_faceOffset+face].m_plane.x,\n" +" facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f);\n" +" const float4 WorldNormal = qtRotate(ornB, Normal);\n" +" float d = dot3F4(WorldNormal,separatingNormal);\n" +" if (d > dmax)\n" +" {\n" +" dmax = d;\n" +" closestFaceB = face;\n" +" }\n" +" }\n" +" }\n" +" \n" +" {\n" +" const btGpuFace polyB = facesB[hullB->m_faceOffset+closestFaceB];\n" +" int numVertices = polyB.m_numIndices;\n" +" if (numVertices>capacityWorldVerts)\n" +" numVertices = capacityWorldVerts;\n" +" \n" +" for(int e0=0;e0m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];\n" +" worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = transform(&b,&posB,&ornB);\n" +" }\n" +" }\n" +" }\n" +" \n" +" int closestFaceA=0;\n" +" {\n" +" float dmin = FLT_MAX;\n" +" for(int face=0;facem_numFaces;face++)\n" +" {\n" +" const float4 Normal = make_float4(\n" +" facesA[hullA->m_faceOffset+face].m_plane.x,\n" +" facesA[hullA->m_faceOffset+face].m_plane.y,\n" +" facesA[hullA->m_faceOffset+face].m_plane.z,\n" +" 0.f);\n" +" const float4 faceANormalWS = qtRotate(ornA,Normal);\n" +" \n" +" float d = dot3F4(faceANormalWS,separatingNormal);\n" +" if (d < dmin)\n" +" {\n" +" dmin = d;\n" +" closestFaceA = face;\n" +" worldNormalsA1[pairIndex] = faceANormalWS;\n" +" }\n" +" }\n" +" }\n" +" \n" +" int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices;\n" +" if (numVerticesA>capacityWorldVerts)\n" +" numVerticesA = capacityWorldVerts;\n" +" \n" +" for(int e0=0;e0m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]];\n" +" worldVertsA1[pairIndex*capacityWorldVerts+e0] = transform(&a, &posA,&ornA);\n" +" }\n" +" }\n" +" \n" +" clippingFaces[pairIndex].x = closestFaceA;\n" +" clippingFaces[pairIndex].y = closestFaceB;\n" +" clippingFaces[pairIndex].z = numVerticesA;\n" +" clippingFaces[pairIndex].w = numWorldVertsB1;\n" +" \n" +" \n" +" return numContactsOut;\n" +"}\n" +"// work-in-progress\n" +"__kernel void findConcaveSeparatingAxisKernel( __global int4* concavePairs,\n" +" __global const BodyData* rigidBodies,\n" +" __global const btCollidableGpu* collidables,\n" +" __global const ConvexPolyhedronCL* convexShapes, \n" +" __global const float4* vertices,\n" +" __global const float4* uniqueEdges,\n" +" __global const btGpuFace* faces,\n" +" __global const int* indices,\n" +" __global const btGpuChildShape* gpuChildShapes,\n" +" __global btAabbCL* aabbs,\n" +" __global float4* concaveSeparatingNormalsOut,\n" +" __global int* concaveHasSeparatingNormals,\n" +" __global int4* clippingFacesOut,\n" +" __global float4* worldVertsA1GPU,\n" +" __global float4* worldNormalsAGPU,\n" +" __global float4* worldVertsB1GPU,\n" +" int vertexFaceCapacity,\n" +" int numConcavePairs\n" +" )\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numConcavePairs)\n" +" return;\n" +" concaveHasSeparatingNormals[i] = 0;\n" +" int pairIdx = i;\n" +" int bodyIndexA = concavePairs[i].x;\n" +" int bodyIndexB = concavePairs[i].y;\n" +" int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;\n" +" int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;\n" +" int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;\n" +" int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;\n" +" if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&&\n" +" collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS)\n" +" {\n" +" concavePairs[pairIdx].w = -1;\n" +" return;\n" +" }\n" +" int numFacesA = convexShapes[shapeIndexA].m_numFaces;\n" +" int numActualConcaveConvexTests = 0;\n" +" \n" +" int f = concavePairs[i].z;\n" +" \n" +" bool overlap = false;\n" +" \n" +" ConvexPolyhedronCL convexPolyhedronA;\n" +" //add 3 vertices of the triangle\n" +" convexPolyhedronA.m_numVertices = 3;\n" +" convexPolyhedronA.m_vertexOffset = 0;\n" +" float4 localCenter = make_float4(0.f,0.f,0.f,0.f);\n" +" btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];\n" +" float4 triMinAabb, triMaxAabb;\n" +" btAabbCL triAabb;\n" +" triAabb.m_min = make_float4(1e30f,1e30f,1e30f,0.f);\n" +" triAabb.m_max = make_float4(-1e30f,-1e30f,-1e30f,0.f);\n" +" \n" +" float4 verticesA[3];\n" +" for (int i=0;i<3;i++)\n" +" {\n" +" int index = indices[face.m_indexOffset+i];\n" +" float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];\n" +" verticesA[i] = vert;\n" +" localCenter += vert;\n" +" \n" +" triAabb.m_min = min(triAabb.m_min,vert); \n" +" triAabb.m_max = max(triAabb.m_max,vert); \n" +" }\n" +" overlap = true;\n" +" overlap = (triAabb.m_min.x > aabbs[bodyIndexB].m_max.x || triAabb.m_max.x < aabbs[bodyIndexB].m_min.x) ? false : overlap;\n" +" overlap = (triAabb.m_min.z > aabbs[bodyIndexB].m_max.z || triAabb.m_max.z < aabbs[bodyIndexB].m_min.z) ? false : overlap;\n" +" overlap = (triAabb.m_min.y > aabbs[bodyIndexB].m_max.y || triAabb.m_max.y < aabbs[bodyIndexB].m_min.y) ? false : overlap;\n" +" \n" +" if (overlap)\n" +" {\n" +" float dmin = FLT_MAX;\n" +" int hasSeparatingAxis=5;\n" +" float4 sepAxis=make_float4(1,2,3,4);\n" +" int localCC=0;\n" +" numActualConcaveConvexTests++;\n" +" //a triangle has 3 unique edges\n" +" convexPolyhedronA.m_numUniqueEdges = 3;\n" +" convexPolyhedronA.m_uniqueEdgesOffset = 0;\n" +" float4 uniqueEdgesA[3];\n" +" \n" +" uniqueEdgesA[0] = (verticesA[1]-verticesA[0]);\n" +" uniqueEdgesA[1] = (verticesA[2]-verticesA[1]);\n" +" uniqueEdgesA[2] = (verticesA[0]-verticesA[2]);\n" +" convexPolyhedronA.m_faceOffset = 0;\n" +" \n" +" float4 normal = make_float4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);\n" +" \n" +" btGpuFace facesA[TRIANGLE_NUM_CONVEX_FACES];\n" +" int indicesA[3+3+2+2+2];\n" +" int curUsedIndices=0;\n" +" int fidx=0;\n" +" //front size of triangle\n" +" {\n" +" facesA[fidx].m_indexOffset=curUsedIndices;\n" +" indicesA[0] = 0;\n" +" indicesA[1] = 1;\n" +" indicesA[2] = 2;\n" +" curUsedIndices+=3;\n" +" float c = face.m_plane.w;\n" +" facesA[fidx].m_plane.x = normal.x;\n" +" facesA[fidx].m_plane.y = normal.y;\n" +" facesA[fidx].m_plane.z = normal.z;\n" +" facesA[fidx].m_plane.w = c;\n" +" facesA[fidx].m_numIndices=3;\n" +" }\n" +" fidx++;\n" +" //back size of triangle\n" +" {\n" +" facesA[fidx].m_indexOffset=curUsedIndices;\n" +" indicesA[3]=2;\n" +" indicesA[4]=1;\n" +" indicesA[5]=0;\n" +" curUsedIndices+=3;\n" +" float c = dot(normal,verticesA[0]);\n" +" float c1 = -face.m_plane.w;\n" +" facesA[fidx].m_plane.x = -normal.x;\n" +" facesA[fidx].m_plane.y = -normal.y;\n" +" facesA[fidx].m_plane.z = -normal.z;\n" +" facesA[fidx].m_plane.w = c;\n" +" facesA[fidx].m_numIndices=3;\n" +" }\n" +" fidx++;\n" +" bool addEdgePlanes = true;\n" +" if (addEdgePlanes)\n" +" {\n" +" int numVertices=3;\n" +" int prevVertex = numVertices-1;\n" +" for (int i=0;i( device, 1, BufferBase::BUFFER_CONST ); + + m_lower = (maxSize == 0)? 0: new b3OpenCLArray(ctx,queue,maxSize ); + m_upper = (maxSize == 0)? 0: new b3OpenCLArray(ctx,queue, maxSize ); + + m_filler = new b3FillCL(ctx,device,queue); +} + +b3BoundSearchCL::~b3BoundSearchCL() +{ + + delete m_lower; + delete m_upper; + delete m_filler; + + clReleaseKernel(m_lowerSortDataKernel); + clReleaseKernel(m_upperSortDataKernel); + clReleaseKernel(m_subtractKernel); + + +} + + +void b3BoundSearchCL::execute(b3OpenCLArray& src, int nSrc, b3OpenCLArray& dst, int nDst, Option option ) +{ + b3Int4 constBuffer; + constBuffer.x = nSrc; + constBuffer.y = nDst; + + if( option == BOUND_LOWER ) + { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( src.getBufferCL(), true ), b3BufferInfoCL( dst.getBufferCL()) }; + + b3LauncherCL launcher( m_queue, m_lowerSortDataKernel,"m_lowerSortDataKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( nSrc ); + launcher.setConst( nDst ); + + launcher.launch1D( nSrc, 64 ); + } + else if( option == BOUND_UPPER ) + { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( src.getBufferCL(), true ), b3BufferInfoCL( dst.getBufferCL() ) }; + + b3LauncherCL launcher(m_queue, m_upperSortDataKernel,"m_upperSortDataKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( nSrc ); + launcher.setConst( nDst ); + + launcher.launch1D( nSrc, 64 ); + } + else if( option == COUNT ) + { + b3Assert( m_lower ); + b3Assert( m_upper ); + b3Assert( m_lower->capacity() <= (int)nDst ); + b3Assert( m_upper->capacity() <= (int)nDst ); + + int zero = 0; + m_filler->execute( *m_lower, zero, nDst ); + m_filler->execute( *m_upper, zero, nDst ); + + execute( src, nSrc, *m_lower, nDst, BOUND_LOWER ); + execute( src, nSrc, *m_upper, nDst, BOUND_UPPER ); + + { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_upper->getBufferCL(), true ), b3BufferInfoCL( m_lower->getBufferCL(), true ), b3BufferInfoCL( dst.getBufferCL() ) }; + + b3LauncherCL launcher( m_queue, m_subtractKernel ,"m_subtractKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( nSrc ); + launcher.setConst( nDst ); + + launcher.launch1D( nDst, 64 ); + } + } + else + { + b3Assert( 0 ); + } + +} + + +void b3BoundSearchCL::executeHost( b3AlignedObjectArray& src, int nSrc, + b3AlignedObjectArray& dst, int nDst, Option option ) +{ + + + for(int i=0; i lower; + lower.resize(nDst ); + b3AlignedObjectArray upper; + upper.resize(nDst ); + + for(int i=0; i +#include +#include +#include +*/ + +#include "b3OpenCLArray.h" +#include "b3FillCL.h" +#include "b3RadixSort32CL.h" //for b3SortData (perhaps move it?) +class b3BoundSearchCL +{ + public: + + enum Option + { + BOUND_LOWER, + BOUND_UPPER, + COUNT, + }; + + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; + + + cl_kernel m_lowerSortDataKernel; + cl_kernel m_upperSortDataKernel; + cl_kernel m_subtractKernel; + + b3OpenCLArray* m_constbtOpenCLArray; + b3OpenCLArray* m_lower; + b3OpenCLArray* m_upper; + + b3FillCL* m_filler; + + b3BoundSearchCL(cl_context context, cl_device_id device, cl_command_queue queue, int size); + + virtual ~b3BoundSearchCL(); + + // src has to be src[i].m_key <= src[i+1].m_key + void execute( b3OpenCLArray& src, int nSrc, b3OpenCLArray& dst, int nDst, Option option = BOUND_LOWER ); + + void executeHost( b3AlignedObjectArray& src, int nSrc, b3AlignedObjectArray& dst, int nDst, Option option = BOUND_LOWER); +}; + + +#endif //B3_BOUNDSEARCH_H diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3BufferInfoCL.h b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3BufferInfoCL.h new file mode 100644 index 000000000000..52f219ae3fc0 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3BufferInfoCL.h @@ -0,0 +1,19 @@ + +#ifndef B3_BUFFER_INFO_CL_H +#define B3_BUFFER_INFO_CL_H + +#include "b3OpenCLArray.h" + + +struct b3BufferInfoCL +{ + //b3BufferInfoCL(){} + +// template + b3BufferInfoCL(cl_mem buff, bool isReadOnly = false): m_clBuffer(buff), m_isReadOnly(isReadOnly){} + + cl_mem m_clBuffer; + bool m_isReadOnly; +}; + +#endif //B3_BUFFER_INFO_CL_H diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.cpp b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.cpp new file mode 100644 index 000000000000..f05c2648f126 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.cpp @@ -0,0 +1,126 @@ +#include "b3FillCL.h" +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "b3BufferInfoCL.h" +#include "b3LauncherCL.h" + +#define FILL_CL_PROGRAM_PATH "src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernels.cl" + +#include "kernels/FillKernelsCL.h" + +b3FillCL::b3FillCL(cl_context ctx, cl_device_id device, cl_command_queue queue) +:m_commandQueue(queue) +{ + const char* kernelSource = fillKernelsCL; + cl_int pErrNum; + const char* additionalMacros = ""; + + cl_program fillProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, kernelSource, &pErrNum,additionalMacros, FILL_CL_PROGRAM_PATH); + b3Assert(fillProg); + + m_fillIntKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, kernelSource, "FillIntKernel", &pErrNum, fillProg,additionalMacros ); + b3Assert(m_fillIntKernel); + + m_fillUnsignedIntKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, kernelSource, "FillUnsignedIntKernel", &pErrNum, fillProg,additionalMacros ); + b3Assert(m_fillIntKernel); + + m_fillFloatKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, kernelSource, "FillFloatKernel", &pErrNum, fillProg,additionalMacros ); + b3Assert(m_fillFloatKernel); + + + + m_fillKernelInt2 = b3OpenCLUtils::compileCLKernelFromString( ctx, device, kernelSource, "FillInt2Kernel", &pErrNum, fillProg,additionalMacros ); + b3Assert(m_fillKernelInt2); + +} + +b3FillCL::~b3FillCL() +{ + clReleaseKernel(m_fillKernelInt2); + clReleaseKernel(m_fillIntKernel); + clReleaseKernel(m_fillUnsignedIntKernel); + clReleaseKernel(m_fillFloatKernel); + +} + +void b3FillCL::execute(b3OpenCLArray& src, const float value, int n, int offset) +{ + b3Assert( n>0 ); + + { + b3LauncherCL launcher( m_commandQueue, m_fillFloatKernel,"m_fillFloatKernel" ); + launcher.setBuffer( src.getBufferCL()); + launcher.setConst( n ); + launcher.setConst( value ); + launcher.setConst( offset); + + launcher.launch1D( n ); + } +} + +void b3FillCL::execute(b3OpenCLArray& src, const int value, int n, int offset) +{ + b3Assert( n>0 ); + + + { + b3LauncherCL launcher( m_commandQueue, m_fillIntKernel ,"m_fillIntKernel"); + launcher.setBuffer(src.getBufferCL()); + launcher.setConst( n); + launcher.setConst( value); + launcher.setConst( offset); + launcher.launch1D( n ); + } +} + + +void b3FillCL::execute(b3OpenCLArray& src, const unsigned int value, int n, int offset) +{ + b3Assert( n>0 ); + + { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( src.getBufferCL() ) }; + + b3LauncherCL launcher( m_commandQueue, m_fillUnsignedIntKernel,"m_fillUnsignedIntKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( n ); + launcher.setConst(value); + launcher.setConst(offset); + + launcher.launch1D( n ); + } +} + +void b3FillCL::executeHost(b3AlignedObjectArray &src, const b3Int2 &value, int n, int offset) +{ + for (int i=0;i &src, const int value, int n, int offset) +{ + for (int i=0;i &src, const b3Int2 &value, int n, int offset) +{ + b3Assert( n>0 ); + + + { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( src.getBufferCL() ) }; + + b3LauncherCL launcher(m_commandQueue, m_fillKernelInt2,"m_fillKernelInt2"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(n); + launcher.setConst(value); + launcher.setConst(offset); + + //( constBuffer ); + launcher.launch1D( n ); + } +} diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.h b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.h new file mode 100644 index 000000000000..1609676b9ddd --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3FillCL.h @@ -0,0 +1,63 @@ +#ifndef B3_FILL_CL_H +#define B3_FILL_CL_H + +#include "b3OpenCLArray.h" +#include "Bullet3Common/b3Scalar.h" + +#include "Bullet3Common/shared/b3Int2.h" +#include "Bullet3Common/shared/b3Int4.h" + + +class b3FillCL +{ + + cl_command_queue m_commandQueue; + + cl_kernel m_fillKernelInt2; + cl_kernel m_fillIntKernel; + cl_kernel m_fillUnsignedIntKernel; + cl_kernel m_fillFloatKernel; + + public: + + struct b3ConstData + { + union + { + b3Int4 m_data; + b3UnsignedInt4 m_UnsignedData; + }; + int m_offset; + int m_n; + int m_padding[2]; + }; + +protected: + +public: + + b3FillCL(cl_context ctx, cl_device_id device, cl_command_queue queue); + + virtual ~b3FillCL(); + + void execute(b3OpenCLArray& src, const unsigned int value, int n, int offset = 0); + + void execute(b3OpenCLArray& src, const int value, int n, int offset = 0); + + void execute(b3OpenCLArray& src, const float value, int n, int offset = 0); + + void execute(b3OpenCLArray& src, const b3Int2& value, int n, int offset = 0); + + void executeHost(b3AlignedObjectArray &src, const b3Int2 &value, int n, int offset); + + void executeHost(b3AlignedObjectArray &src, const int value, int n, int offset); + + // void execute(b3OpenCLArray& src, const b3Int4& value, int n, int offset = 0); + +}; + + + + + +#endif //B3_FILL_CL_H diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.cpp b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.cpp new file mode 100644 index 000000000000..94590d11caf4 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.cpp @@ -0,0 +1,308 @@ +#include "b3LauncherCL.h" + +bool gDebugLauncherCL = false; + +b3LauncherCL::b3LauncherCL(cl_command_queue queue, cl_kernel kernel, const char* name) +:m_commandQueue(queue), +m_kernel(kernel), +m_idx(0), +m_enableSerialization(false), +m_name(name) +{ + if (gDebugLauncherCL) + { + static int counter = 0; + printf("[%d] Prepare to launch OpenCL kernel %s\n", counter++, name); + } + + m_serializationSizeInBytes = sizeof(int); +} + +b3LauncherCL::~b3LauncherCL() + { + for (int i=0;i + + + +int b3LauncherCL::deserializeArgs(unsigned char* buf, int bufSize, cl_context ctx) +{ + int index=0; + + int numArguments = *(int*) &buf[index]; + index+=sizeof(int); + + for (int i=0;im_isBuffer) + { + b3OpenCLArray* clData = new b3OpenCLArray(ctx,m_commandQueue, arg->m_argSizeInBytes); + clData->resize(arg->m_argSizeInBytes); + + clData->copyFromHostPointer(&buf[index], arg->m_argSizeInBytes); + + arg->m_clBuffer = clData->getBufferCL(); + + m_arrays.push_back(clData); + + cl_int status = clSetKernelArg( m_kernel, m_idx++, sizeof(cl_mem), &arg->m_clBuffer); + b3Assert( status == CL_SUCCESS ); + index+=arg->m_argSizeInBytes; + } else + { + cl_int status = clSetKernelArg( m_kernel, m_idx++, arg->m_argSizeInBytes, &arg->m_argData); + b3Assert( status == CL_SUCCESS ); + } + b3KernelArgData b; + memcpy(&b,arg,sizeof(b3KernelArgDataUnaligned)); + m_kernelArguments.push_back(b); + } +m_serializationSizeInBytes = index; + return index; +} + +int b3LauncherCL::validateResults(unsigned char* goldBuffer, int goldBufferCapacity, cl_context ctx) + { + int index=0; + + int numArguments = *(int*) &goldBuffer[index]; + index+=sizeof(int); + + if (numArguments != m_kernelArguments.size()) + { + printf("failed validation: expected %d arguments, found %d\n",numArguments, m_kernelArguments.size()); + return -1; + } + + for (int ii=0;iim_argSizeInBytes) + { + printf("failed validation: argument %d sizeInBytes expected: %d, found %d\n",ii, argGold->m_argSizeInBytes, m_kernelArguments[ii].m_argSizeInBytes); + return -2; + } + + { + int expected = argGold->m_isBuffer; + int found = m_kernelArguments[ii].m_isBuffer; + + if (expected != found) + { + printf("failed validation: argument %d isBuffer expected: %d, found %d\n",ii,expected, found); + return -3; + } + } + index+=sizeof(b3KernelArgData); + + if (argGold->m_isBuffer) + { + + unsigned char* memBuf= (unsigned char*) malloc(m_kernelArguments[ii].m_argSizeInBytes); + unsigned char* goldBuf = &goldBuffer[index]; + for (int j=0;jm_argSizeInBytes; + } else + { + + //compare content + for (int b=0;bm_argData[b]; + int found =m_kernelArguments[ii].m_argData[b]; + if (expected != found) + { + printf("failed validation: argument %d const data at byte position %d expected: %d, found %d\n", + ii, b, expected, found); + return -5; + } + } + + } + } + return index; + +} + +int b3LauncherCL::serializeArguments(unsigned char* destBuffer, int destBufferCapacity) +{ +//initialize to known values +for (int i=0;i=m_serializationSizeInBytes); + + //todo: use the b3Serializer for this to allow for 32/64bit, endianness etc + int numArguments = m_kernelArguments.size(); + int curBufferSize = 0; + int* dest = (int*)&destBuffer[curBufferSize]; + *dest = numArguments; + curBufferSize += sizeof(int); + + + + for (int i=0;im_kernelArguments.size();i++) + { + b3KernelArgData* arg = (b3KernelArgData*) &destBuffer[curBufferSize]; + *arg = m_kernelArguments[i]; + curBufferSize+=sizeof(b3KernelArgData); + if (arg->m_isBuffer==1) + { + //copy the OpenCL buffer content + cl_int status = 0; + status = clEnqueueReadBuffer( m_commandQueue, arg->m_clBuffer, 0, 0, arg->m_argSizeInBytes, + &destBuffer[curBufferSize], 0,0,0 ); + b3Assert( status==CL_SUCCESS ); + clFinish(m_commandQueue); + curBufferSize+=arg->m_argSizeInBytes; + } + + } + return curBufferSize; +} + +void b3LauncherCL::serializeToFile(const char* fileName, int numWorkItems) +{ + int num = numWorkItems; + int buffSize = getSerializationBufferSize(); + unsigned char* buf = new unsigned char[buffSize+sizeof(int)]; + for (int i=0;i + +#define B3_DEBUG_SERIALIZE_CL + + +#ifdef _WIN32 +#pragma warning(disable :4996) +#endif +#define B3_CL_MAX_ARG_SIZE 16 +B3_ATTRIBUTE_ALIGNED16(struct) b3KernelArgData +{ + int m_isBuffer; + int m_argIndex; + int m_argSizeInBytes; + int m_unusedPadding; + union + { + cl_mem m_clBuffer; + unsigned char m_argData[B3_CL_MAX_ARG_SIZE]; + }; + +}; + +class b3LauncherCL +{ + + cl_command_queue m_commandQueue; + cl_kernel m_kernel; + int m_idx; + + b3AlignedObjectArray m_kernelArguments; + int m_serializationSizeInBytes; + bool m_enableSerialization; + + const char* m_name; + public: + + b3AlignedObjectArray* > m_arrays; + + b3LauncherCL(cl_command_queue queue, cl_kernel kernel, const char* name); + + virtual ~b3LauncherCL(); + + void setBuffer( cl_mem clBuffer); + + void setBuffers( b3BufferInfoCL* buffInfo, int n ); + + int getSerializationBufferSize() const + { + return m_serializationSizeInBytes; + } + + int deserializeArgs(unsigned char* buf, int bufSize, cl_context ctx); + + inline int validateResults(unsigned char* goldBuffer, int goldBufferCapacity, cl_context ctx); + + int serializeArguments(unsigned char* destBuffer, int destBufferCapacity); + + int getNumArguments() const + { + return m_kernelArguments.size(); + } + + b3KernelArgData getArgument(int index) + { + return m_kernelArguments[index]; + } + + void serializeToFile(const char* fileName, int numWorkItems); + + template + inline void setConst( const T& consts ) + { + int sz=sizeof(T); + b3Assert(sz<=B3_CL_MAX_ARG_SIZE); + + if (m_enableSerialization) + { + b3KernelArgData kernelArg; + kernelArg.m_argIndex = m_idx; + kernelArg.m_isBuffer = 0; + T* destArg = (T*)kernelArg.m_argData; + *destArg = consts; + kernelArg.m_argSizeInBytes = sizeof(T); + m_kernelArguments.push_back(kernelArg); + m_serializationSizeInBytes+=sizeof(b3KernelArgData); + } + + cl_int status = clSetKernelArg( m_kernel, m_idx++, sz, &consts ); + b3Assert( status == CL_SUCCESS ); + } + + inline void launch1D( int numThreads, int localSize = 64) + { + launch2D( numThreads, 1, localSize, 1 ); + } + + inline void launch2D( int numThreadsX, int numThreadsY, int localSizeX, int localSizeY ) + { + size_t gRange[3] = {1,1,1}; + size_t lRange[3] = {1,1,1}; + lRange[0] = localSizeX; + lRange[1] = localSizeY; + gRange[0] = b3Max((size_t)1, (numThreadsX/lRange[0])+(!(numThreadsX%lRange[0])?0:1)); + gRange[0] *= lRange[0]; + gRange[1] = b3Max((size_t)1, (numThreadsY/lRange[1])+(!(numThreadsY%lRange[1])?0:1)); + gRange[1] *= lRange[1]; + + cl_int status = clEnqueueNDRangeKernel( m_commandQueue, + m_kernel, 2, NULL, gRange, lRange, 0,0,0 ); + if (status != CL_SUCCESS) + { + printf("Error: OpenCL status = %d\n",status); + } + b3Assert( status == CL_SUCCESS ); + + } + + void enableSerialization(bool serialize) + { + m_enableSerialization = serialize; + } + +}; + + + +#endif //B3_LAUNCHER_CL_H diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h new file mode 100644 index 000000000000..d70c30f53f4a --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h @@ -0,0 +1,306 @@ +#ifndef B3_OPENCL_ARRAY_H +#define B3_OPENCL_ARRAY_H + +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" + +template +class b3OpenCLArray +{ + size_t m_size; + size_t m_capacity; + cl_mem m_clBuffer; + + cl_context m_clContext; + cl_command_queue m_commandQueue; + + bool m_ownsMemory; + + bool m_allowGrowingCapacity; + + void deallocate() + { + if (m_clBuffer && m_ownsMemory) + { + clReleaseMemObject(m_clBuffer); + } + m_clBuffer = 0; + m_capacity=0; + } + + b3OpenCLArray& operator=(const b3OpenCLArray& src); + + B3_FORCE_INLINE size_t allocSize(size_t size) + { + return (size ? size*2 : 1); + } + +public: + + b3OpenCLArray(cl_context ctx, cl_command_queue queue, size_t initialCapacity=0, bool allowGrowingCapacity=true) + :m_size(0), m_capacity(0),m_clBuffer(0), + m_clContext(ctx),m_commandQueue(queue), + m_ownsMemory(true),m_allowGrowingCapacity(true) + { + if (initialCapacity) + { + reserve(initialCapacity); + } + m_allowGrowingCapacity = allowGrowingCapacity; + } + + ///this is an error-prone method with no error checking, be careful! + void setFromOpenCLBuffer(cl_mem buffer, size_t sizeInElements) + { + deallocate(); + m_ownsMemory = false; + m_allowGrowingCapacity = false; + m_clBuffer = buffer; + m_size = sizeInElements; + m_capacity = sizeInElements; + } + +// we could enable this assignment, but need to make sure to avoid accidental deep copies +// b3OpenCLArray& operator=(const b3AlignedObjectArray& src) +// { +// copyFromArray(src); +// return *this; +// } + + + cl_mem getBufferCL() const + { + return m_clBuffer; + } + + + virtual ~b3OpenCLArray() + { + deallocate(); + m_size=0; + m_capacity=0; + } + + B3_FORCE_INLINE bool push_back(const T& _Val,bool waitForCompletion=true) + { + bool result = true; + size_t sz = size(); + if( sz == capacity() ) + { + result = reserve( allocSize(size()) ); + } + copyFromHostPointer(&_Val, 1, sz, waitForCompletion); + m_size++; + return result; + } + + B3_FORCE_INLINE T forcedAt(size_t n) const + { + b3Assert(n>=0); + b3Assert(n=0); + b3Assert(n size()) + { + result = reserve(newsize,copyOldContents); + } + + //leave new data uninitialized (init in debug mode?) + //for (size_t i=curSize;i0); + b3Assert(numElements<=m_size); + + size_t srcOffsetBytes = sizeof(T)*firstElem; + size_t dstOffsetInBytes = sizeof(T)*dstOffsetInElems; + + status = clEnqueueCopyBuffer( m_commandQueue, m_clBuffer, destination, + srcOffsetBytes, dstOffsetInBytes, sizeof(T)*numElements, 0, 0, 0 ); + + b3Assert( status == CL_SUCCESS ); + } + + void copyFromHost(const b3AlignedObjectArray& srcArray, bool waitForCompletion=true) + { + size_t newSize = srcArray.size(); + + bool copyOldContents = false; + resize (newSize,copyOldContents); + if (newSize) + copyFromHostPointer(&srcArray[0],newSize,0,waitForCompletion); + + } + + void copyFromHostPointer(const T* src, size_t numElems, size_t destFirstElem= 0, bool waitForCompletion=true) + { + b3Assert(numElems+destFirstElem <= capacity()); + + if (numElems+destFirstElem) + { + cl_int status = 0; + size_t sizeInBytes=sizeof(T)*numElems; + status = clEnqueueWriteBuffer( m_commandQueue, m_clBuffer, 0, sizeof(T)*destFirstElem, sizeInBytes, + src, 0,0,0 ); + b3Assert(status == CL_SUCCESS ); + if (waitForCompletion) + clFinish(m_commandQueue); + } else + { + b3Error("copyFromHostPointer invalid range\n"); + } + } + + + void copyToHost(b3AlignedObjectArray& destArray, bool waitForCompletion=true) const + { + destArray.resize(this->size()); + if (size()) + copyToHostPointer(&destArray[0], size(),0,waitForCompletion); + } + + void copyToHostPointer(T* destPtr, size_t numElem, size_t srcFirstElem=0, bool waitForCompletion=true) const + { + b3Assert(numElem+srcFirstElem <= capacity()); + + if(numElem+srcFirstElem <= capacity()) + { + cl_int status = 0; + status = clEnqueueReadBuffer( m_commandQueue, m_clBuffer, 0, sizeof(T)*srcFirstElem, sizeof(T)*numElem, + destPtr, 0,0,0 ); + b3Assert( status==CL_SUCCESS ); + + if (waitForCompletion) + clFinish(m_commandQueue); + } else + { + b3Error("copyToHostPointer invalid range\n"); + } + } + + void copyFromOpenCLArray(const b3OpenCLArray& src) + { + size_t newSize = src.size(); + resize(newSize); + if (size()) + { + src.copyToCL(m_clBuffer,size()); + } + } + +}; + + +#endif //B3_OPENCL_ARRAY_H diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.cpp b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.cpp new file mode 100644 index 000000000000..42cd197740a4 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.cpp @@ -0,0 +1,126 @@ +#include "b3PrefixScanCL.h" +#include "b3FillCL.h" +#define B3_PREFIXSCAN_PROG_PATH "src/Bullet3OpenCL/ParallelPrimitives/kernels/PrefixScanKernels.cl" + +#include "b3LauncherCL.h" +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "kernels/PrefixScanKernelsCL.h" + +b3PrefixScanCL::b3PrefixScanCL(cl_context ctx, cl_device_id device, cl_command_queue queue, int size) +:m_commandQueue(queue) +{ + const char* scanKernelSource = prefixScanKernelsCL; + cl_int pErrNum; + char* additionalMacros=0; + + m_workBuffer = new b3OpenCLArray(ctx,queue,size); + cl_program scanProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, scanKernelSource, &pErrNum,additionalMacros, B3_PREFIXSCAN_PROG_PATH); + b3Assert(scanProg); + + m_localScanKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, scanKernelSource, "LocalScanKernel", &pErrNum, scanProg,additionalMacros ); + b3Assert(m_localScanKernel ); + m_blockSumKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, scanKernelSource, "TopLevelScanKernel", &pErrNum, scanProg,additionalMacros ); + b3Assert(m_blockSumKernel ); + m_propagationKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, scanKernelSource, "AddOffsetKernel", &pErrNum, scanProg,additionalMacros ); + b3Assert(m_propagationKernel ); +} + + +b3PrefixScanCL::~b3PrefixScanCL() +{ + delete m_workBuffer; + clReleaseKernel(m_localScanKernel); + clReleaseKernel(m_blockSumKernel); + clReleaseKernel(m_propagationKernel); +} + +template +T b3NextPowerOf2(T n) +{ + n -= 1; + for(int i=0; i>i); + return n+1; +} + +void b3PrefixScanCL::execute(b3OpenCLArray& src, b3OpenCLArray& dst, int n, unsigned int* sum) +{ + +// b3Assert( data->m_option == EXCLUSIVE ); + const unsigned int numBlocks = (const unsigned int)( (n+BLOCK_SIZE*2-1)/(BLOCK_SIZE*2) ); + + dst.resize(src.size()); + m_workBuffer->resize(src.size()); + + b3Int4 constBuffer; + constBuffer.x = n; + constBuffer.y = numBlocks; + constBuffer.z = (int)b3NextPowerOf2( numBlocks ); + + b3OpenCLArray* srcNative = &src; + b3OpenCLArray* dstNative = &dst; + + { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( dstNative->getBufferCL() ), b3BufferInfoCL( srcNative->getBufferCL() ), b3BufferInfoCL( m_workBuffer->getBufferCL() ) }; + + b3LauncherCL launcher( m_commandQueue, m_localScanKernel,"m_localScanKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( constBuffer ); + launcher.launch1D( numBlocks*BLOCK_SIZE, BLOCK_SIZE ); + } + + { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_workBuffer->getBufferCL() ) }; + + b3LauncherCL launcher( m_commandQueue, m_blockSumKernel,"m_blockSumKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( constBuffer ); + launcher.launch1D( BLOCK_SIZE, BLOCK_SIZE ); + } + + + if( numBlocks > 1 ) + { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( dstNative->getBufferCL() ), b3BufferInfoCL( m_workBuffer->getBufferCL() ) }; + b3LauncherCL launcher( m_commandQueue, m_propagationKernel,"m_propagationKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( constBuffer ); + launcher.launch1D( (numBlocks-1)*BLOCK_SIZE, BLOCK_SIZE ); + } + + + if( sum ) + { + clFinish(m_commandQueue); + dstNative->copyToHostPointer(sum,1,n-1,true); + } + +} + + +void b3PrefixScanCL::executeHost(b3AlignedObjectArray& src, b3AlignedObjectArray& dst, int n, unsigned int* sum) +{ + unsigned int s = 0; + //if( data->m_option == EXCLUSIVE ) + { + for(int i=0; i* m_workBuffer; + + + public: + + b3PrefixScanCL(cl_context ctx, cl_device_id device, cl_command_queue queue,int size=0); + + virtual ~b3PrefixScanCL(); + + void execute(b3OpenCLArray& src, b3OpenCLArray& dst, int n, unsigned int* sum = 0); + void executeHost(b3AlignedObjectArray& src, b3AlignedObjectArray& dst, int n, unsigned int* sum=0); +}; + +#endif //B3_PREFIX_SCAN_CL_H diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanFloat4CL.cpp b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanFloat4CL.cpp new file mode 100644 index 000000000000..80560d793de4 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3PrefixScanFloat4CL.cpp @@ -0,0 +1,126 @@ +#include "b3PrefixScanFloat4CL.h" +#include "b3FillCL.h" +#define B3_PREFIXSCAN_FLOAT4_PROG_PATH "src/Bullet3OpenCL/ParallelPrimitives/kernels/PrefixScanFloat4Kernels.cl" + +#include "b3LauncherCL.h" +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "kernels/PrefixScanKernelsFloat4CL.h" + +b3PrefixScanFloat4CL::b3PrefixScanFloat4CL(cl_context ctx, cl_device_id device, cl_command_queue queue, int size) +:m_commandQueue(queue) +{ + const char* scanKernelSource = prefixScanKernelsFloat4CL; + cl_int pErrNum; + char* additionalMacros=0; + + m_workBuffer = new b3OpenCLArray(ctx,queue,size); + cl_program scanProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, scanKernelSource, &pErrNum,additionalMacros, B3_PREFIXSCAN_FLOAT4_PROG_PATH); + b3Assert(scanProg); + + m_localScanKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, scanKernelSource, "LocalScanKernel", &pErrNum, scanProg,additionalMacros ); + b3Assert(m_localScanKernel ); + m_blockSumKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, scanKernelSource, "TopLevelScanKernel", &pErrNum, scanProg,additionalMacros ); + b3Assert(m_blockSumKernel ); + m_propagationKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, scanKernelSource, "AddOffsetKernel", &pErrNum, scanProg,additionalMacros ); + b3Assert(m_propagationKernel ); +} + + +b3PrefixScanFloat4CL::~b3PrefixScanFloat4CL() +{ + delete m_workBuffer; + clReleaseKernel(m_localScanKernel); + clReleaseKernel(m_blockSumKernel); + clReleaseKernel(m_propagationKernel); +} + +template +T b3NextPowerOf2(T n) +{ + n -= 1; + for(int i=0; i>i); + return n+1; +} + +void b3PrefixScanFloat4CL::execute(b3OpenCLArray& src, b3OpenCLArray& dst, int n, b3Vector3* sum) +{ + +// b3Assert( data->m_option == EXCLUSIVE ); + const unsigned int numBlocks = (const unsigned int)( (n+BLOCK_SIZE*2-1)/(BLOCK_SIZE*2) ); + + dst.resize(src.size()); + m_workBuffer->resize(src.size()); + + b3Int4 constBuffer; + constBuffer.x = n; + constBuffer.y = numBlocks; + constBuffer.z = (int)b3NextPowerOf2( numBlocks ); + + b3OpenCLArray* srcNative = &src; + b3OpenCLArray* dstNative = &dst; + + { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( dstNative->getBufferCL() ), b3BufferInfoCL( srcNative->getBufferCL() ), b3BufferInfoCL( m_workBuffer->getBufferCL() ) }; + + b3LauncherCL launcher( m_commandQueue, m_localScanKernel ,"m_localScanKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( constBuffer ); + launcher.launch1D( numBlocks*BLOCK_SIZE, BLOCK_SIZE ); + } + + { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_workBuffer->getBufferCL() ) }; + + b3LauncherCL launcher( m_commandQueue, m_blockSumKernel ,"m_blockSumKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( constBuffer ); + launcher.launch1D( BLOCK_SIZE, BLOCK_SIZE ); + } + + + if( numBlocks > 1 ) + { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( dstNative->getBufferCL() ), b3BufferInfoCL( m_workBuffer->getBufferCL() ) }; + b3LauncherCL launcher( m_commandQueue, m_propagationKernel ,"m_propagationKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( constBuffer ); + launcher.launch1D( (numBlocks-1)*BLOCK_SIZE, BLOCK_SIZE ); + } + + + if( sum ) + { + clFinish(m_commandQueue); + dstNative->copyToHostPointer(sum,1,n-1,true); + } + +} + + +void b3PrefixScanFloat4CL::executeHost(b3AlignedObjectArray& src, b3AlignedObjectArray& dst, int n, b3Vector3* sum) +{ + b3Vector3 s=b3MakeVector3(0,0,0); + //if( data->m_option == EXCLUSIVE ) + { + for(int i=0; i* m_workBuffer; + + + public: + + b3PrefixScanFloat4CL(cl_context ctx, cl_device_id device, cl_command_queue queue,int size=0); + + virtual ~b3PrefixScanFloat4CL(); + + void execute(b3OpenCLArray& src, b3OpenCLArray& dst, int n, b3Vector3* sum = 0); + void executeHost(b3AlignedObjectArray& src, b3AlignedObjectArray& dst, int n, b3Vector3* sum); +}; + +#endif //B3_PREFIX_SCAN_CL_H diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.cpp b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.cpp new file mode 100644 index 000000000000..f11ae4bcdb6e --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.cpp @@ -0,0 +1,710 @@ + +#include "b3RadixSort32CL.h" +#include "b3LauncherCL.h" +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "b3PrefixScanCL.h" +#include "b3FillCL.h" + +#define RADIXSORT32_PATH "src/Bullet3OpenCL/ParallelPrimitives/kernels/RadixSort32Kernels.cl" + +#include "kernels/RadixSort32KernelsCL.h" + +b3RadixSort32CL::b3RadixSort32CL(cl_context ctx, cl_device_id device, cl_command_queue queue, int initialCapacity) +:m_commandQueue(queue) +{ + b3OpenCLDeviceInfo info; + b3OpenCLUtils::getDeviceInfo(device,&info); + m_deviceCPU = (info.m_deviceType & CL_DEVICE_TYPE_CPU)!=0; + + m_workBuffer1 = new b3OpenCLArray(ctx,queue); + m_workBuffer2 = new b3OpenCLArray(ctx,queue); + m_workBuffer3 = new b3OpenCLArray(ctx,queue); + m_workBuffer3a = new b3OpenCLArray(ctx,queue); + m_workBuffer4 = new b3OpenCLArray(ctx,queue); + m_workBuffer4a = new b3OpenCLArray(ctx,queue); + + + if (initialCapacity>0) + { + m_workBuffer1->resize(initialCapacity); + m_workBuffer3->resize(initialCapacity); + m_workBuffer3a->resize(initialCapacity); + m_workBuffer4->resize(initialCapacity); + m_workBuffer4a->resize(initialCapacity); + } + + m_scan = new b3PrefixScanCL(ctx,device,queue); + m_fill = new b3FillCL(ctx,device,queue); + + const char* additionalMacros = ""; + + cl_int pErrNum; + const char* kernelSource = radixSort32KernelsCL; + + cl_program sortProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, kernelSource, &pErrNum,additionalMacros, RADIXSORT32_PATH); + b3Assert(sortProg); + + m_streamCountSortDataKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, kernelSource, "StreamCountSortDataKernel", &pErrNum, sortProg,additionalMacros ); + b3Assert(m_streamCountSortDataKernel ); + + + + m_streamCountKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, kernelSource, "StreamCountKernel", &pErrNum, sortProg,additionalMacros ); + b3Assert(m_streamCountKernel); + + + + if (m_deviceCPU) + { + + m_sortAndScatterSortDataKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, kernelSource, "SortAndScatterSortDataKernelSerial", &pErrNum, sortProg,additionalMacros ); + b3Assert(m_sortAndScatterSortDataKernel); + m_sortAndScatterKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, kernelSource, "SortAndScatterKernelSerial", &pErrNum, sortProg,additionalMacros ); + b3Assert(m_sortAndScatterKernel); + } else + { + m_sortAndScatterSortDataKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, kernelSource, "SortAndScatterSortDataKernel", &pErrNum, sortProg,additionalMacros ); + b3Assert(m_sortAndScatterSortDataKernel); + m_sortAndScatterKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, kernelSource, "SortAndScatterKernel", &pErrNum, sortProg,additionalMacros ); + b3Assert(m_sortAndScatterKernel); + } + + m_prefixScanKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, kernelSource, "PrefixScanKernel", &pErrNum, sortProg,additionalMacros ); + b3Assert(m_prefixScanKernel); + +} + +b3RadixSort32CL::~b3RadixSort32CL() +{ + delete m_scan; + delete m_fill; + delete m_workBuffer1; + delete m_workBuffer2; + delete m_workBuffer3; + delete m_workBuffer3a; + delete m_workBuffer4; + delete m_workBuffer4a; + + clReleaseKernel(m_streamCountSortDataKernel); + clReleaseKernel(m_streamCountKernel); + clReleaseKernel(m_sortAndScatterSortDataKernel); + clReleaseKernel(m_sortAndScatterKernel); + clReleaseKernel(m_prefixScanKernel); +} + +void b3RadixSort32CL::executeHost(b3AlignedObjectArray& inout, int sortBits /* = 32 */) +{ + int n = inout.size(); + const int BITS_PER_PASS = 8; + const int NUM_TABLES = (1< workbuffer; + workbuffer.resize(inout.size()); + b3SortData* dst = &workbuffer[0]; + + int count=0; + for(int startBit=0; startBit> startBit) & (NUM_TABLES-1); + tables[tableIdx]++; + } +//#define TEST +#ifdef TEST + printf("histogram size=%d\n",NUM_TABLES); + for (int i=0;i> startBit) & (NUM_TABLES-1); + + dst[tables[tableIdx] + counter[tableIdx]] = src[i]; + counter[tableIdx] ++; + } + + b3Swap( src, dst ); + count++; + } + + if (count&1) + { + b3Assert(0);//need to copy + + } +} + +void b3RadixSort32CL::executeHost(b3OpenCLArray& keyValuesInOut, int sortBits /* = 32 */) +{ + + b3AlignedObjectArray inout; + keyValuesInOut.copyToHost(inout); + + executeHost(inout,sortBits); + + keyValuesInOut.copyFromHost(inout); +} + +void b3RadixSort32CL::execute(b3OpenCLArray& keysIn, b3OpenCLArray& keysOut, b3OpenCLArray& valuesIn, + b3OpenCLArray& valuesOut, int n, int sortBits) +{ + +} + +//#define DEBUG_RADIXSORT +//#define DEBUG_RADIXSORT2 + + +void b3RadixSort32CL::execute(b3OpenCLArray& keyValuesInOut, int sortBits /* = 32 */) +{ + + int originalSize = keyValuesInOut.size(); + int workingSize = originalSize; + + + int dataAlignment = DATA_ALIGNMENT; + +#ifdef DEBUG_RADIXSORT2 + b3AlignedObjectArray test2; + keyValuesInOut.copyToHost(test2); + printf("numElem = %d\n",test2.size()); + for (int i=0;i* src = 0; + + if (workingSize%dataAlignment) + { + workingSize += dataAlignment-(workingSize%dataAlignment); + m_workBuffer4->copyFromOpenCLArray(keyValuesInOut); + m_workBuffer4->resize(workingSize); + b3SortData fillValue; + fillValue.m_key = 0xffffffff; + fillValue.m_value = 0xffffffff; + +#define USE_BTFILL +#ifdef USE_BTFILL + m_fill->execute((b3OpenCLArray&)*m_workBuffer4,(b3Int2&)fillValue,workingSize-originalSize,originalSize); +#else + //fill the remaining bits (very slow way, todo: fill on GPU/OpenCL side) + + for (int i=originalSize; icopyFromHostPointer(&fillValue,1,i); + } +#endif//USE_BTFILL + + src = m_workBuffer4; + } else + { + src = &keyValuesInOut; + m_workBuffer4->resize(0); + } + + b3Assert( workingSize%DATA_ALIGNMENT == 0 ); + int minCap = NUM_BUCKET*NUM_WGS; + + + int n = workingSize; + + m_workBuffer1->resize(minCap); + m_workBuffer3->resize(workingSize); + + +// ADLASSERT( ELEMENTS_PER_WORK_ITEM == 4 ); + b3Assert( BITS_PER_PASS == 4 ); + b3Assert( WG_SIZE == 64 ); + b3Assert( (sortBits&0x3) == 0 ); + + + + b3OpenCLArray* dst = m_workBuffer3; + + b3OpenCLArray* srcHisto = m_workBuffer1; + b3OpenCLArray* destHisto = m_workBuffer2; + + + int nWGs = NUM_WGS; + b3ConstData cdata; + + { + int blockSize = ELEMENTS_PER_WORK_ITEM*WG_SIZE;//set at 256 + int nBlocks = (n+blockSize-1)/(blockSize); + cdata.m_n = n; + cdata.m_nWGs = NUM_WGS; + cdata.m_startBit = 0; + cdata.m_nBlocksPerWG = (nBlocks + cdata.m_nWGs - 1)/cdata.m_nWGs; + if( nBlocks < NUM_WGS ) + { + cdata.m_nBlocksPerWG = 1; + nWGs = nBlocks; + } + } + + int count=0; + for(int ib=0; ibsize()) + { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( src->getBufferCL(), true ), b3BufferInfoCL( srcHisto->getBufferCL() ) }; + b3LauncherCL launcher(m_commandQueue, m_streamCountSortDataKernel,"m_streamCountSortDataKernel"); + + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( cdata ); + + int num = NUM_WGS*WG_SIZE; + launcher.launch1D( num, WG_SIZE ); + } + + + +#ifdef DEBUG_RADIXSORT + b3AlignedObjectArray testHist; + srcHisto->copyToHost(testHist); + printf("ib = %d, testHist size = %d, non zero elements:\n",ib, testHist.size()); + for (int i=0;igetBufferCL() ) }; + b3LauncherCL launcher( m_commandQueue, m_prefixScanKernel,"m_prefixScanKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( cdata ); + launcher.launch1D( 128, 128 ); + destHisto = srcHisto; + }else + { + //unsigned int sum; //for debugging + m_scan->execute(*srcHisto,*destHisto,1920,0);//,&sum); + } + + +#ifdef DEBUG_RADIXSORT + destHisto->copyToHost(testHist); + printf("ib = %d, testHist size = %d, non zero elements:\n",ib, testHist.size()); + for (int i=0;isize()) + {// local sort and distribute + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( src->getBufferCL(), true ), b3BufferInfoCL( destHisto->getBufferCL(), true ), b3BufferInfoCL( dst->getBufferCL() )}; + b3LauncherCL launcher( m_commandQueue, m_sortAndScatterSortDataKernel,"m_sortAndScatterSortDataKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( cdata ); + launcher.launch1D( nWGs*WG_SIZE, WG_SIZE ); + + } +#else + { +#define NUM_TABLES 16 +//#define SEQUENTIAL +#ifdef SEQUENTIAL + int counter2[NUM_TABLES]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; + int tables[NUM_TABLES]; + int startBit = ib; + + destHisto->copyToHost(testHist); + b3AlignedObjectArray srcHost; + b3AlignedObjectArray dstHost; + dstHost.resize(src->size()); + + src->copyToHost(srcHost); + + for (int i=0;i> startBit) & (NUM_TABLES-1); + + dstHost[tables[tableIdx] + counter2[tableIdx]] = srcHost[i]; + counter2[tableIdx] ++; + } + + +#else + + int counter2[NUM_TABLES]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; + + int tables[NUM_TABLES]; + b3AlignedObjectArray dstHostOK; + dstHostOK.resize(src->size()); + + destHisto->copyToHost(testHist); + b3AlignedObjectArray srcHost; + src->copyToHost(srcHost); + + int blockSize = 256; + int nBlocksPerWG = cdata.m_nBlocksPerWG; + int startBit = ib; + + { + for (int i=0;i> startBit) & (NUM_TABLES-1); + + dstHostOK[tables[tableIdx] + counter2[tableIdx]] = srcHost[i]; + counter2[tableIdx] ++; + } + + + } + + + b3AlignedObjectArray dstHost; + dstHost.resize(src->size()); + + + int counter[NUM_TABLES]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; + + + + for (int wgIdx=0;wgIdx> startBit) & (NUM_TABLES-1); + + int destIndex = testHist[tableIdx*NUM_WGS+wgIdx] + counter[tableIdx]; + + b3SortData ok = dstHostOK[destIndex]; + + if (ok.m_key != srcHost[i].m_key) + { + printf("ok.m_key = %d, srcHost[i].m_key = %d\n", ok.m_key,srcHost[i].m_key ); + printf("(ok.m_value = %d, srcHost[i].m_value = %d)\n", ok.m_value,srcHost[i].m_value ); + } + if (ok.m_value != srcHost[i].m_value) + { + + printf("ok.m_value = %d, srcHost[i].m_value = %d\n", ok.m_value,srcHost[i].m_value ); + printf("(ok.m_key = %d, srcHost[i].m_key = %d)\n", ok.m_key,srcHost[i].m_key ); + + } + + dstHost[destIndex] = srcHost[i]; + counter[tableIdx] ++; + + } + } + } + } + } + + +#endif //SEQUENTIAL + + dst->copyFromHost(dstHost); + } +#endif//USE_GPU + + + +#ifdef DEBUG_RADIXSORT + destHisto->copyToHost(testHist); + printf("ib = %d, testHist size = %d, non zero elements:\n",ib, testHist.size()); + for (int i=0;isize()) + { + m_workBuffer4->resize(originalSize); + keyValuesInOut.copyFromOpenCLArray(*m_workBuffer4); + } + + +#ifdef DEBUG_RADIXSORT + keyValuesInOut.copyToHost(test2); + + printf("numElem = %d\n",test2.size()); + for (int i=0;i& keysInOut, int sortBits /* = 32 */) +{ + int originalSize = keysInOut.size(); + int workingSize = originalSize; + + + int dataAlignment = DATA_ALIGNMENT; + + b3OpenCLArray* src = 0; + + if (workingSize%dataAlignment) + { + workingSize += dataAlignment-(workingSize%dataAlignment); + m_workBuffer4a->copyFromOpenCLArray(keysInOut); + m_workBuffer4a->resize(workingSize); + unsigned int fillValue = 0xffffffff; + + m_fill->execute(*m_workBuffer4a,fillValue,workingSize-originalSize,originalSize); + + src = m_workBuffer4a; + } else + { + src = &keysInOut; + m_workBuffer4a->resize(0); + } + + + + b3Assert( workingSize%DATA_ALIGNMENT == 0 ); + int minCap = NUM_BUCKET*NUM_WGS; + + + int n = workingSize; + + + m_workBuffer1->resize(minCap); + m_workBuffer3->resize(workingSize); + m_workBuffer3a->resize(workingSize); + +// ADLASSERT( ELEMENTS_PER_WORK_ITEM == 4 ); + b3Assert( BITS_PER_PASS == 4 ); + b3Assert( WG_SIZE == 64 ); + b3Assert( (sortBits&0x3) == 0 ); + + + + b3OpenCLArray* dst = m_workBuffer3a; + + b3OpenCLArray* srcHisto = m_workBuffer1; + b3OpenCLArray* destHisto = m_workBuffer2; + + + int nWGs = NUM_WGS; + b3ConstData cdata; + + { + int blockSize = ELEMENTS_PER_WORK_ITEM*WG_SIZE;//set at 256 + int nBlocks = (n+blockSize-1)/(blockSize); + cdata.m_n = n; + cdata.m_nWGs = NUM_WGS; + cdata.m_startBit = 0; + cdata.m_nBlocksPerWG = (nBlocks + cdata.m_nWGs - 1)/cdata.m_nWGs; + if( nBlocks < NUM_WGS ) + { + cdata.m_nBlocksPerWG = 1; + nWGs = nBlocks; + } + } + + int count=0; + for(int ib=0; ibsize()) + { + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( src->getBufferCL(), true ), b3BufferInfoCL( srcHisto->getBufferCL() ) }; + b3LauncherCL launcher(m_commandQueue, m_streamCountKernel,"m_streamCountKernel"); + + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( cdata ); + + int num = NUM_WGS*WG_SIZE; + launcher.launch1D( num, WG_SIZE ); + } + + + +//fast prefix scan is not working properly on Mac OSX yet +#ifdef __APPLE__ + bool fastScan=false; +#else + bool fastScan=!m_deviceCPU; +#endif + + if (fastScan) + {// prefix scan group histogram + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( srcHisto->getBufferCL() ) }; + b3LauncherCL launcher( m_commandQueue, m_prefixScanKernel,"m_prefixScanKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( cdata ); + launcher.launch1D( 128, 128 ); + destHisto = srcHisto; + }else + { + //unsigned int sum; //for debugging + m_scan->execute(*srcHisto,*destHisto,1920,0);//,&sum); + } + + if (src->size()) + {// local sort and distribute + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( src->getBufferCL(), true ), b3BufferInfoCL( destHisto->getBufferCL(), true ), b3BufferInfoCL( dst->getBufferCL() )}; + b3LauncherCL launcher( m_commandQueue, m_sortAndScatterKernel ,"m_sortAndScatterKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( cdata ); + launcher.launch1D( nWGs*WG_SIZE, WG_SIZE ); + + } + + b3Swap(src, dst ); + b3Swap(srcHisto,destHisto); + + count++; + } + + if (count&1) + { + b3Assert(0);//need to copy from workbuffer to keyValuesInOut + } + + if (m_workBuffer4a->size()) + { + m_workBuffer4a->resize(originalSize); + keysInOut.copyFromOpenCLArray(*m_workBuffer4a); + } + +} + + + + + + + diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h new file mode 100644 index 000000000000..975bd80e5307 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h @@ -0,0 +1,95 @@ + +#ifndef B3_RADIXSORT32_H +#define B3_RADIXSORT32_H + +#include "b3OpenCLArray.h" + +struct b3SortData +{ + union + { + unsigned int m_key; + unsigned int x; + }; + + union + { + unsigned int m_value; + unsigned int y; + + }; +}; +#include "b3BufferInfoCL.h" + +class b3RadixSort32CL +{ + + b3OpenCLArray* m_workBuffer1; + b3OpenCLArray* m_workBuffer2; + + b3OpenCLArray* m_workBuffer3; + b3OpenCLArray* m_workBuffer4; + + b3OpenCLArray* m_workBuffer3a; + b3OpenCLArray* m_workBuffer4a; + + cl_command_queue m_commandQueue; + + cl_kernel m_streamCountSortDataKernel; + cl_kernel m_streamCountKernel; + + cl_kernel m_prefixScanKernel; + cl_kernel m_sortAndScatterSortDataKernel; + cl_kernel m_sortAndScatterKernel; + + + bool m_deviceCPU; + + class b3PrefixScanCL* m_scan; + class b3FillCL* m_fill; + +public: + struct b3ConstData + { + int m_n; + int m_nWGs; + int m_startBit; + int m_nBlocksPerWG; + }; + enum + { + DATA_ALIGNMENT = 256, + WG_SIZE = 64, + BLOCK_SIZE = 256, + ELEMENTS_PER_WORK_ITEM = (BLOCK_SIZE/WG_SIZE), + BITS_PER_PASS = 4, + NUM_BUCKET=(1<& keysIn, b3OpenCLArray& keysOut, b3OpenCLArray& valuesIn, + b3OpenCLArray& valuesOut, int n, int sortBits = 32); + + ///keys only + void execute(b3OpenCLArray& keysInOut, int sortBits = 32 ); + + void execute(b3OpenCLArray& keyValuesInOut, int sortBits = 32 ); + void executeHost(b3OpenCLArray& keyValuesInOut, int sortBits = 32); + void executeHost(b3AlignedObjectArray& keyValuesInOut, int sortBits = 32); + +}; +#endif //B3_RADIXSORT32_H + diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernels.cl b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernels.cl new file mode 100644 index 000000000000..f3b4a1e8a795 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernels.cl @@ -0,0 +1,106 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, 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. +*/ +//Originally written by Takahiro Harada + + +typedef unsigned int u32; +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) + +typedef struct +{ + u32 m_key; + u32 m_value; +}SortData; + + + +typedef struct +{ + u32 m_nSrc; + u32 m_nDst; + u32 m_padding[2]; +} ConstBuffer; + + + +__attribute__((reqd_work_group_size(64,1,1))) +__kernel +void SearchSortDataLowerKernel(__global SortData* src, __global u32 *dst, + unsigned int nSrc, unsigned int nDst) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nSrc ) + { + SortData first; first.m_key = (u32)(-1); first.m_value = (u32)(-1); + SortData end; end.m_key = nDst; end.m_value = nDst; + + SortData iData = (gIdx==0)? first: src[gIdx-1]; + SortData jData = (gIdx==nSrc)? end: src[gIdx]; + + if( iData.m_key != jData.m_key ) + { +// for(u32 k=iData.m_key+1; k<=min(jData.m_key, nDst-1); k++) + u32 k = jData.m_key; + { + dst[k] = gIdx; + } + } + } +} + + +__attribute__((reqd_work_group_size(64,1,1))) +__kernel +void SearchSortDataUpperKernel(__global SortData* src, __global u32 *dst, + unsigned int nSrc, unsigned int nDst) +{ + int gIdx = GET_GLOBAL_IDX+1; + + if( gIdx < nSrc+1 ) + { + SortData first; first.m_key = 0; first.m_value = 0; + SortData end; end.m_key = nDst; end.m_value = nDst; + + SortData iData = src[gIdx-1]; + SortData jData = (gIdx==nSrc)? end: src[gIdx]; + + if( iData.m_key != jData.m_key ) + { + u32 k = iData.m_key; + { + dst[k] = gIdx; + } + } + } +} + +__attribute__((reqd_work_group_size(64,1,1))) +__kernel +void SubtractKernel(__global u32* A, __global u32 *B, __global u32 *C, + unsigned int nSrc, unsigned int nDst) +{ + int gIdx = GET_GLOBAL_IDX; + + + if( gIdx < nDst ) + { + C[gIdx] = A[gIdx] - B[gIdx]; + } +} + diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernelsCL.h b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernelsCL.h new file mode 100644 index 000000000000..9c9e847138b6 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/BoundSearchKernelsCL.h @@ -0,0 +1,87 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* boundSearchKernelsCL= \ +"/*\n" +"Copyright (c) 2012 Advanced Micro Devices, Inc. \n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose, \n" +"including commercial applications, and to alter it and redistribute it freely, \n" +"subject to the following restrictions:\n" +"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.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"//Originally written by Takahiro Harada\n" +"typedef unsigned int u32;\n" +"#define GET_GROUP_IDX get_group_id(0)\n" +"#define GET_LOCAL_IDX get_local_id(0)\n" +"#define GET_GLOBAL_IDX get_global_id(0)\n" +"#define GET_GROUP_SIZE get_local_size(0)\n" +"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" +"typedef struct\n" +"{\n" +" u32 m_key; \n" +" u32 m_value;\n" +"}SortData;\n" +"typedef struct\n" +"{\n" +" u32 m_nSrc;\n" +" u32 m_nDst;\n" +" u32 m_padding[2];\n" +"} ConstBuffer;\n" +"__attribute__((reqd_work_group_size(64,1,1)))\n" +"__kernel\n" +"void SearchSortDataLowerKernel(__global SortData* src, __global u32 *dst, \n" +" unsigned int nSrc, unsigned int nDst)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" if( gIdx < nSrc )\n" +" {\n" +" SortData first; first.m_key = (u32)(-1); first.m_value = (u32)(-1);\n" +" SortData end; end.m_key = nDst; end.m_value = nDst;\n" +" SortData iData = (gIdx==0)? first: src[gIdx-1];\n" +" SortData jData = (gIdx==nSrc)? end: src[gIdx];\n" +" if( iData.m_key != jData.m_key )\n" +" {\n" +"// for(u32 k=iData.m_key+1; k<=min(jData.m_key, nDst-1); k++)\n" +" u32 k = jData.m_key;\n" +" {\n" +" dst[k] = gIdx;\n" +" }\n" +" }\n" +" }\n" +"}\n" +"__attribute__((reqd_work_group_size(64,1,1)))\n" +"__kernel\n" +"void SearchSortDataUpperKernel(__global SortData* src, __global u32 *dst, \n" +" unsigned int nSrc, unsigned int nDst)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX+1;\n" +" if( gIdx < nSrc+1 )\n" +" {\n" +" SortData first; first.m_key = 0; first.m_value = 0;\n" +" SortData end; end.m_key = nDst; end.m_value = nDst;\n" +" SortData iData = src[gIdx-1];\n" +" SortData jData = (gIdx==nSrc)? end: src[gIdx];\n" +" if( iData.m_key != jData.m_key )\n" +" {\n" +" u32 k = iData.m_key;\n" +" {\n" +" dst[k] = gIdx;\n" +" }\n" +" }\n" +" }\n" +"}\n" +"__attribute__((reqd_work_group_size(64,1,1)))\n" +"__kernel\n" +"void SubtractKernel(__global u32* A, __global u32 *B, __global u32 *C, \n" +" unsigned int nSrc, unsigned int nDst)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" \n" +" if( gIdx < nDst )\n" +" {\n" +" C[gIdx] = A[gIdx] - B[gIdx];\n" +" }\n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernels.cl b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernels.cl new file mode 100644 index 000000000000..2eee5752ecfb --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernels.cl @@ -0,0 +1,128 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, 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. +*/ +//Originally written by Takahiro Harada + +#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable + +typedef unsigned int u32; +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) + +#define make_uint4 (uint4) +#define make_uint2 (uint2) +#define make_int2 (int2) + +typedef struct +{ + int m_n; + int m_padding[3]; +} ConstBuffer; + + + +__kernel +__attribute__((reqd_work_group_size(64,1,1))) +void Copy1F4Kernel(__global float4* dst, __global float4* src, + ConstBuffer cb) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < cb.m_n ) + { + float4 a0 = src[gIdx]; + + dst[ gIdx ] = a0; + } +} + +__kernel +__attribute__((reqd_work_group_size(64,1,1))) +void Copy2F4Kernel(__global float4* dst, __global float4* src, + ConstBuffer cb) +{ + int gIdx = GET_GLOBAL_IDX; + + if( 2*gIdx <= cb.m_n ) + { + float4 a0 = src[gIdx*2+0]; + float4 a1 = src[gIdx*2+1]; + + dst[ gIdx*2+0 ] = a0; + dst[ gIdx*2+1 ] = a1; + } +} + +__kernel +__attribute__((reqd_work_group_size(64,1,1))) +void Copy4F4Kernel(__global float4* dst, __global float4* src, + ConstBuffer cb) +{ + int gIdx = GET_GLOBAL_IDX; + + if( 4*gIdx <= cb.m_n ) + { + int idx0 = gIdx*4+0; + int idx1 = gIdx*4+1; + int idx2 = gIdx*4+2; + int idx3 = gIdx*4+3; + + float4 a0 = src[idx0]; + float4 a1 = src[idx1]; + float4 a2 = src[idx2]; + float4 a3 = src[idx3]; + + dst[ idx0 ] = a0; + dst[ idx1 ] = a1; + dst[ idx2 ] = a2; + dst[ idx3 ] = a3; + } +} + +__kernel +__attribute__((reqd_work_group_size(64,1,1))) +void CopyF1Kernel(__global float* dstF1, __global float* srcF1, + ConstBuffer cb) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < cb.m_n ) + { + float a0 = srcF1[gIdx]; + + dstF1[ gIdx ] = a0; + } +} + +__kernel +__attribute__((reqd_work_group_size(64,1,1))) +void CopyF2Kernel(__global float2* dstF2, __global float2* srcF2, + ConstBuffer cb) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < cb.m_n ) + { + float2 a0 = srcF2[gIdx]; + + dstF2[ gIdx ] = a0; + } +} + diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernelsCL.h b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernelsCL.h new file mode 100644 index 000000000000..e5670e3cd3dd --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/CopyKernelsCL.h @@ -0,0 +1,132 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* copyKernelsCL= \ +"/*\n" +"Copyright (c) 2012 Advanced Micro Devices, Inc. \n" +"\n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose, \n" +"including commercial applications, and to alter it and redistribute it freely, \n" +"subject to the following restrictions:\n" +"\n" +"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.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"//Originally written by Takahiro Harada\n" +"\n" +"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" +"\n" +"typedef unsigned int u32;\n" +"#define GET_GROUP_IDX get_group_id(0)\n" +"#define GET_LOCAL_IDX get_local_id(0)\n" +"#define GET_GLOBAL_IDX get_global_id(0)\n" +"#define GET_GROUP_SIZE get_local_size(0)\n" +"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" +"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" +"#define AtomInc(x) atom_inc(&(x))\n" +"#define AtomInc1(x, out) out = atom_inc(&(x))\n" +"\n" +"#define make_uint4 (uint4)\n" +"#define make_uint2 (uint2)\n" +"#define make_int2 (int2)\n" +"\n" +"typedef struct\n" +"{\n" +" int m_n;\n" +" int m_padding[3];\n" +"} ConstBuffer;\n" +"\n" +"\n" +"\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(64,1,1)))\n" +"void Copy1F4Kernel(__global float4* dst, __global float4* src, \n" +" ConstBuffer cb)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +"\n" +" if( gIdx < cb.m_n )\n" +" {\n" +" float4 a0 = src[gIdx];\n" +"\n" +" dst[ gIdx ] = a0;\n" +" }\n" +"}\n" +"\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(64,1,1)))\n" +"void Copy2F4Kernel(__global float4* dst, __global float4* src, \n" +" ConstBuffer cb)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +"\n" +" if( 2*gIdx <= cb.m_n )\n" +" {\n" +" float4 a0 = src[gIdx*2+0];\n" +" float4 a1 = src[gIdx*2+1];\n" +"\n" +" dst[ gIdx*2+0 ] = a0;\n" +" dst[ gIdx*2+1 ] = a1;\n" +" }\n" +"}\n" +"\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(64,1,1)))\n" +"void Copy4F4Kernel(__global float4* dst, __global float4* src, \n" +" ConstBuffer cb)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +"\n" +" if( 4*gIdx <= cb.m_n )\n" +" {\n" +" int idx0 = gIdx*4+0;\n" +" int idx1 = gIdx*4+1;\n" +" int idx2 = gIdx*4+2;\n" +" int idx3 = gIdx*4+3;\n" +"\n" +" float4 a0 = src[idx0];\n" +" float4 a1 = src[idx1];\n" +" float4 a2 = src[idx2];\n" +" float4 a3 = src[idx3];\n" +"\n" +" dst[ idx0 ] = a0;\n" +" dst[ idx1 ] = a1;\n" +" dst[ idx2 ] = a2;\n" +" dst[ idx3 ] = a3;\n" +" }\n" +"}\n" +"\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(64,1,1)))\n" +"void CopyF1Kernel(__global float* dstF1, __global float* srcF1, \n" +" ConstBuffer cb)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +"\n" +" if( gIdx < cb.m_n )\n" +" {\n" +" float a0 = srcF1[gIdx];\n" +"\n" +" dstF1[ gIdx ] = a0;\n" +" }\n" +"}\n" +"\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(64,1,1)))\n" +"void CopyF2Kernel(__global float2* dstF2, __global float2* srcF2, \n" +" ConstBuffer cb)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +"\n" +" if( gIdx < cb.m_n )\n" +" {\n" +" float2 a0 = srcF2[gIdx];\n" +"\n" +" dstF2[ gIdx ] = a0;\n" +" }\n" +"}\n" +"\n" +"\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernels.cl b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernels.cl new file mode 100644 index 000000000000..71c31075dd76 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernels.cl @@ -0,0 +1,107 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, 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. +*/ +//Originally written by Takahiro Harada + + +#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable + +typedef unsigned int u32; +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) + +#define make_uint4 (uint4) +#define make_uint2 (uint2) +#define make_int2 (int2) + +typedef struct +{ + union + { + int4 m_data; + uint4 m_unsignedData; + float m_floatData; + }; + int m_offset; + int m_n; + int m_padding[2]; +} ConstBuffer; + + +__kernel +__attribute__((reqd_work_group_size(64,1,1))) +void FillIntKernel(__global int* dstInt, int num_elements, int value, const int offset) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < num_elements ) + { + dstInt[ offset+gIdx ] = value; + } +} + +__kernel +__attribute__((reqd_work_group_size(64,1,1))) +void FillFloatKernel(__global float* dstFloat, int num_elements, float value, const int offset) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < num_elements ) + { + dstFloat[ offset+gIdx ] = value; + } +} + +__kernel +__attribute__((reqd_work_group_size(64,1,1))) +void FillUnsignedIntKernel(__global unsigned int* dstInt, const int num, const unsigned int value, const int offset) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < num ) + { + dstInt[ offset+gIdx ] = value; + } +} + +__kernel +__attribute__((reqd_work_group_size(64,1,1))) +void FillInt2Kernel(__global int2* dstInt2, const int num, const int2 value, const int offset) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < num ) + { + dstInt2[ gIdx + offset] = make_int2( value.x, value.y ); + } +} + +__kernel +__attribute__((reqd_work_group_size(64,1,1))) +void FillInt4Kernel(__global int4* dstInt4, const int num, const int4 value, const int offset) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < num ) + { + dstInt4[ offset+gIdx ] = value; + } +} + diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernelsCL.h b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernelsCL.h new file mode 100644 index 000000000000..4f8b96e48954 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/FillKernelsCL.h @@ -0,0 +1,91 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* fillKernelsCL= \ +"/*\n" +"Copyright (c) 2012 Advanced Micro Devices, Inc. \n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose, \n" +"including commercial applications, and to alter it and redistribute it freely, \n" +"subject to the following restrictions:\n" +"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.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"//Originally written by Takahiro Harada\n" +"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" +"typedef unsigned int u32;\n" +"#define GET_GROUP_IDX get_group_id(0)\n" +"#define GET_LOCAL_IDX get_local_id(0)\n" +"#define GET_GLOBAL_IDX get_global_id(0)\n" +"#define GET_GROUP_SIZE get_local_size(0)\n" +"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" +"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" +"#define AtomInc(x) atom_inc(&(x))\n" +"#define AtomInc1(x, out) out = atom_inc(&(x))\n" +"#define make_uint4 (uint4)\n" +"#define make_uint2 (uint2)\n" +"#define make_int2 (int2)\n" +"typedef struct\n" +"{\n" +" union\n" +" {\n" +" int4 m_data;\n" +" uint4 m_unsignedData;\n" +" float m_floatData;\n" +" };\n" +" int m_offset;\n" +" int m_n;\n" +" int m_padding[2];\n" +"} ConstBuffer;\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(64,1,1)))\n" +"void FillIntKernel(__global int* dstInt, int num_elements, int value, const int offset)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" if( gIdx < num_elements )\n" +" {\n" +" dstInt[ offset+gIdx ] = value;\n" +" }\n" +"}\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(64,1,1)))\n" +"void FillFloatKernel(__global float* dstFloat, int num_elements, float value, const int offset)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" if( gIdx < num_elements )\n" +" {\n" +" dstFloat[ offset+gIdx ] = value;\n" +" }\n" +"}\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(64,1,1)))\n" +"void FillUnsignedIntKernel(__global unsigned int* dstInt, const int num, const unsigned int value, const int offset)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" if( gIdx < num )\n" +" {\n" +" dstInt[ offset+gIdx ] = value;\n" +" }\n" +"}\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(64,1,1)))\n" +"void FillInt2Kernel(__global int2* dstInt2, const int num, const int2 value, const int offset)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" if( gIdx < num )\n" +" {\n" +" dstInt2[ gIdx + offset] = make_int2( value.x, value.y );\n" +" }\n" +"}\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(64,1,1)))\n" +"void FillInt4Kernel(__global int4* dstInt4, const int num, const int4 value, const int offset)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" if( gIdx < num )\n" +" {\n" +" dstInt4[ offset+gIdx ] = value;\n" +" }\n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/PrefixScanFloat4Kernels.cl b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/PrefixScanFloat4Kernels.cl new file mode 100644 index 000000000000..c9da79854a2f --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/PrefixScanFloat4Kernels.cl @@ -0,0 +1,154 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, 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. +*/ +//Originally written by Takahiro Harada + + +typedef unsigned int u32; +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) + +// takahiro end +#define WG_SIZE 128 +#define m_numElems x +#define m_numBlocks y +#define m_numScanBlocks z + +/*typedef struct +{ + uint m_numElems; + uint m_numBlocks; + uint m_numScanBlocks; + uint m_padding[1]; +} ConstBuffer; +*/ + +float4 ScanExclusiveFloat4(__local float4* data, u32 n, int lIdx, int lSize) +{ + float4 blocksum; + int offset = 1; + for(int nActive=n>>1; nActive>0; nActive>>=1, offset<<=1) + { + GROUP_LDS_BARRIER; + for(int iIdx=lIdx; iIdx>= 1; + for(int nActive=1; nActive>=1 ) + { + GROUP_LDS_BARRIER; + for( int iIdx = lIdx; iIdx>1; nActive>0; nActive>>=1, offset<<=1) + { + GROUP_LDS_BARRIER; + for(int iIdx=lIdx; iIdx>= 1; + for(int nActive=1; nActive>=1 ) + { + GROUP_LDS_BARRIER; + for( int iIdx = lIdx; iIdx>1; nActive>0; nActive>>=1, offset<<=1)\n" +" {\n" +" GROUP_LDS_BARRIER;\n" +" for(int iIdx=lIdx; iIdx>= 1;\n" +" for(int nActive=1; nActive>=1 )\n" +" {\n" +" GROUP_LDS_BARRIER;\n" +" for( int iIdx = lIdx; iIdx>1; nActive>0; nActive>>=1, offset<<=1)\n" +" {\n" +" GROUP_LDS_BARRIER;\n" +" for(int iIdx=lIdx; iIdx>= 1;\n" +" for(int nActive=1; nActive>=1 )\n" +" {\n" +" GROUP_LDS_BARRIER;\n" +" for( int iIdx = lIdx; iIdx 64 ) + { + sorterSharedMemory[idx] += sorterSharedMemory[idx-64]; + GROUP_MEM_FENCE; + } + + sorterSharedMemory[idx-1] += sorterSharedMemory[idx-2]; + GROUP_MEM_FENCE; + } +#else + if( lIdx < 64 ) + { + sorterSharedMemory[idx] += sorterSharedMemory[idx-1]; + GROUP_MEM_FENCE; + sorterSharedMemory[idx] += sorterSharedMemory[idx-2]; + GROUP_MEM_FENCE; + sorterSharedMemory[idx] += sorterSharedMemory[idx-4]; + GROUP_MEM_FENCE; + sorterSharedMemory[idx] += sorterSharedMemory[idx-8]; + GROUP_MEM_FENCE; + sorterSharedMemory[idx] += sorterSharedMemory[idx-16]; + GROUP_MEM_FENCE; + sorterSharedMemory[idx] += sorterSharedMemory[idx-32]; + GROUP_MEM_FENCE; + if( wgSize > 64 ) + { + sorterSharedMemory[idx] += sorterSharedMemory[idx-64]; + GROUP_MEM_FENCE; + } + + sorterSharedMemory[idx-1] += sorterSharedMemory[idx-2]; + GROUP_MEM_FENCE; + } +#endif + } + + GROUP_LDS_BARRIER; + + *totalSum = sorterSharedMemory[wgSize*2-1]; + u32 addValue = sorterSharedMemory[lIdx+wgSize-1]; + return addValue; +} + +//__attribute__((reqd_work_group_size(128,1,1))) +uint4 localPrefixSum128V( uint4 pData, uint lIdx, uint* totalSum, __local u32* sorterSharedMemory ) +{ + u32 s4 = prefixScanVectorEx( &pData ); + u32 rank = localPrefixSum( s4, lIdx, totalSum, sorterSharedMemory, 128 ); + return pData + make_uint4( rank, rank, rank, rank ); +} + + +//__attribute__((reqd_work_group_size(64,1,1))) +uint4 localPrefixSum64V( uint4 pData, uint lIdx, uint* totalSum, __local u32* sorterSharedMemory ) +{ + u32 s4 = prefixScanVectorEx( &pData ); + u32 rank = localPrefixSum( s4, lIdx, totalSum, sorterSharedMemory, 64 ); + return pData + make_uint4( rank, rank, rank, rank ); +} + +u32 unpack4Key( u32 key, int keyIdx ){ return (key>>(keyIdx*8)) & 0xff;} + +u32 bit8Scan(u32 v) +{ + return (v<<8) + (v<<16) + (v<<24); +} + +//=== + + + + +#define MY_HISTOGRAM(idx) localHistogramMat[(idx)*WG_SIZE+lIdx] + + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void StreamCountKernel( __global u32* gSrc, __global u32* histogramOut, int4 cb ) +{ + __local u32 localHistogramMat[NUM_BUCKET*WG_SIZE]; + + u32 gIdx = GET_GLOBAL_IDX; + u32 lIdx = GET_LOCAL_IDX; + u32 wgIdx = GET_GROUP_IDX; + u32 wgSize = GET_GROUP_SIZE; + const int startBit = cb.m_startBit; + const int n = cb.m_n; + const int nWGs = cb.m_nWGs; + const int nBlocksPerWG = cb.m_nBlocksPerWG; + + for(int i=0; i>startBit) & 0xf; +#if defined(NV_GPU) + MY_HISTOGRAM( localKey )++; +#else + AtomInc( MY_HISTOGRAM( localKey ) ); +#endif + } + } + } + + GROUP_LDS_BARRIER; + + if( lIdx < NUM_BUCKET ) + { + u32 sum = 0; + for(int i=0; i>startBit) & 0xf; +#if defined(NV_GPU) + MY_HISTOGRAM( localKey )++; +#else + AtomInc( MY_HISTOGRAM( localKey ) ); +#endif + } + } + } + + GROUP_LDS_BARRIER; + + if( lIdx < NUM_BUCKET ) + { + u32 sum = 0; + for(int i=0; i>startBit) & mask, (sortData[1]>>startBit) & mask, (sortData[2]>>startBit) & mask, (sortData[3]>>startBit) & mask ); + uint4 prefixSum = SELECT_UINT4( make_uint4(1,1,1,1), make_uint4(0,0,0,0), cmpResult != make_uint4(0,0,0,0) ); + u32 total; + prefixSum = localPrefixSum64V( prefixSum, lIdx, &total, ldsSortData ); + { + uint4 localAddr = make_uint4(lIdx*4+0,lIdx*4+1,lIdx*4+2,lIdx*4+3); + uint4 dstAddr = localAddr - prefixSum + make_uint4( total, total, total, total ); + dstAddr = SELECT_UINT4( prefixSum, dstAddr, cmpResult != make_uint4(0, 0, 0, 0) ); + + GROUP_LDS_BARRIER; + + ldsSortData[dstAddr.x] = sortData[0]; + ldsSortData[dstAddr.y] = sortData[1]; + ldsSortData[dstAddr.z] = sortData[2]; + ldsSortData[dstAddr.w] = sortData[3]; + + GROUP_LDS_BARRIER; + + sortData[0] = ldsSortData[localAddr.x]; + sortData[1] = ldsSortData[localAddr.y]; + sortData[2] = ldsSortData[localAddr.z]; + sortData[3] = ldsSortData[localAddr.w]; + + GROUP_LDS_BARRIER; + } + } +} + +// 2 scan, 2 exchange +void sort4Bits1(u32 sortData[4], int startBit, int lIdx, __local u32* ldsSortData) +{ + for(uint ibit=0; ibit>(startBit+ibit)) & 0x3, + (sortData[1]>>(startBit+ibit)) & 0x3, + (sortData[2]>>(startBit+ibit)) & 0x3, + (sortData[3]>>(startBit+ibit)) & 0x3); + + u32 key4; + u32 sKeyPacked[4] = { 0, 0, 0, 0 }; + { + sKeyPacked[0] |= 1<<(8*b.x); + sKeyPacked[1] |= 1<<(8*b.y); + sKeyPacked[2] |= 1<<(8*b.z); + sKeyPacked[3] |= 1<<(8*b.w); + + key4 = sKeyPacked[0] + sKeyPacked[1] + sKeyPacked[2] + sKeyPacked[3]; + } + + u32 rankPacked; + u32 sumPacked; + { + rankPacked = localPrefixSum( key4, lIdx, &sumPacked, ldsSortData, WG_SIZE ); + } + + GROUP_LDS_BARRIER; + + u32 newOffset[4] = { 0,0,0,0 }; + { + u32 sumScanned = bit8Scan( sumPacked ); + + u32 scannedKeys[4]; + scannedKeys[0] = 1<<(8*b.x); + scannedKeys[1] = 1<<(8*b.y); + scannedKeys[2] = 1<<(8*b.z); + scannedKeys[3] = 1<<(8*b.w); + { // 4 scans at once + u32 sum4 = 0; + for(int ie=0; ie<4; ie++) + { + u32 tmp = scannedKeys[ie]; + scannedKeys[ie] = sum4; + sum4 += tmp; + } + } + + { + u32 sumPlusRank = sumScanned + rankPacked; + { u32 ie = b.x; + scannedKeys[0] += sumPlusRank; + newOffset[0] = unpack4Key( scannedKeys[0], ie ); + } + { u32 ie = b.y; + scannedKeys[1] += sumPlusRank; + newOffset[1] = unpack4Key( scannedKeys[1], ie ); + } + { u32 ie = b.z; + scannedKeys[2] += sumPlusRank; + newOffset[2] = unpack4Key( scannedKeys[2], ie ); + } + { u32 ie = b.w; + scannedKeys[3] += sumPlusRank; + newOffset[3] = unpack4Key( scannedKeys[3], ie ); + } + } + } + + + GROUP_LDS_BARRIER; + + { + ldsSortData[newOffset[0]] = sortData[0]; + ldsSortData[newOffset[1]] = sortData[1]; + ldsSortData[newOffset[2]] = sortData[2]; + ldsSortData[newOffset[3]] = sortData[3]; + + GROUP_LDS_BARRIER; + + u32 dstAddr = 4*lIdx; + sortData[0] = ldsSortData[dstAddr+0]; + sortData[1] = ldsSortData[dstAddr+1]; + sortData[2] = ldsSortData[dstAddr+2]; + sortData[3] = ldsSortData[dstAddr+3]; + + GROUP_LDS_BARRIER; + } + } +} + +#define SET_HISTOGRAM(setIdx, key) ldsSortData[(setIdx)*NUM_BUCKET+key] + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void SortAndScatterKernel( __global const u32* restrict gSrc, __global const u32* rHistogram, __global u32* restrict gDst, int4 cb ) +{ + __local u32 ldsSortData[WG_SIZE*ELEMENTS_PER_WORK_ITEM+16]; + __local u32 localHistogramToCarry[NUM_BUCKET]; + __local u32 localHistogram[NUM_BUCKET*2]; + + u32 gIdx = GET_GLOBAL_IDX; + u32 lIdx = GET_LOCAL_IDX; + u32 wgIdx = GET_GROUP_IDX; + u32 wgSize = GET_GROUP_SIZE; + + const int n = cb.m_n; + const int nWGs = cb.m_nWGs; + const int startBit = cb.m_startBit; + const int nBlocksPerWG = cb.m_nBlocksPerWG; + + if( lIdx < (NUM_BUCKET) ) + { + localHistogramToCarry[lIdx] = rHistogram[lIdx*nWGs + wgIdx]; + } + + GROUP_LDS_BARRIER; + + const int blockSize = ELEMENTS_PER_WORK_ITEM*WG_SIZE; + + int nBlocks = n/blockSize - nBlocksPerWG*wgIdx; + + int addr = blockSize*nBlocksPerWG*wgIdx + ELEMENTS_PER_WORK_ITEM*lIdx; + + for(int iblock=0; iblock>startBit) & 0xf; + + { // create histogram + u32 setIdx = lIdx/16; + if( lIdx < NUM_BUCKET ) + { + localHistogram[lIdx] = 0; + } + ldsSortData[lIdx] = 0; + GROUP_LDS_BARRIER; + + for(int i=0; i>(startBit+ibit)) & 0x3, + (sortData[1]>>(startBit+ibit)) & 0x3, + (sortData[2]>>(startBit+ibit)) & 0x3, + (sortData[3]>>(startBit+ibit)) & 0x3); + + u32 key4; + u32 sKeyPacked[4] = { 0, 0, 0, 0 }; + { + sKeyPacked[0] |= 1<<(8*b.x); + sKeyPacked[1] |= 1<<(8*b.y); + sKeyPacked[2] |= 1<<(8*b.z); + sKeyPacked[3] |= 1<<(8*b.w); + + key4 = sKeyPacked[0] + sKeyPacked[1] + sKeyPacked[2] + sKeyPacked[3]; + } + + u32 rankPacked; + u32 sumPacked; + { + rankPacked = localPrefixSum( key4, lIdx, &sumPacked, ldsSortData, WG_SIZE ); + } + + GROUP_LDS_BARRIER; + + u32 newOffset[4] = { 0,0,0,0 }; + { + u32 sumScanned = bit8Scan( sumPacked ); + + u32 scannedKeys[4]; + scannedKeys[0] = 1<<(8*b.x); + scannedKeys[1] = 1<<(8*b.y); + scannedKeys[2] = 1<<(8*b.z); + scannedKeys[3] = 1<<(8*b.w); + { // 4 scans at once + u32 sum4 = 0; + for(int ie=0; ie<4; ie++) + { + u32 tmp = scannedKeys[ie]; + scannedKeys[ie] = sum4; + sum4 += tmp; + } + } + + { + u32 sumPlusRank = sumScanned + rankPacked; + { u32 ie = b.x; + scannedKeys[0] += sumPlusRank; + newOffset[0] = unpack4Key( scannedKeys[0], ie ); + } + { u32 ie = b.y; + scannedKeys[1] += sumPlusRank; + newOffset[1] = unpack4Key( scannedKeys[1], ie ); + } + { u32 ie = b.z; + scannedKeys[2] += sumPlusRank; + newOffset[2] = unpack4Key( scannedKeys[2], ie ); + } + { u32 ie = b.w; + scannedKeys[3] += sumPlusRank; + newOffset[3] = unpack4Key( scannedKeys[3], ie ); + } + } + } + + + GROUP_LDS_BARRIER; + + { + ldsSortData[newOffset[0]] = sortData[0]; + ldsSortData[newOffset[1]] = sortData[1]; + ldsSortData[newOffset[2]] = sortData[2]; + ldsSortData[newOffset[3]] = sortData[3]; + + ldsSortVal[newOffset[0]] = sortVal[0]; + ldsSortVal[newOffset[1]] = sortVal[1]; + ldsSortVal[newOffset[2]] = sortVal[2]; + ldsSortVal[newOffset[3]] = sortVal[3]; + + GROUP_LDS_BARRIER; + + u32 dstAddr = 4*lIdx; + sortData[0] = ldsSortData[dstAddr+0]; + sortData[1] = ldsSortData[dstAddr+1]; + sortData[2] = ldsSortData[dstAddr+2]; + sortData[3] = ldsSortData[dstAddr+3]; + + sortVal[0] = ldsSortVal[dstAddr+0]; + sortVal[1] = ldsSortVal[dstAddr+1]; + sortVal[2] = ldsSortVal[dstAddr+2]; + sortVal[3] = ldsSortVal[dstAddr+3]; + + GROUP_LDS_BARRIER; + } + } +} + + + + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void SortAndScatterSortDataKernel( __global const SortDataCL* restrict gSrc, __global const u32* rHistogram, __global SortDataCL* restrict gDst, int4 cb) +{ + __local int ldsSortData[WG_SIZE*ELEMENTS_PER_WORK_ITEM+16]; + __local int ldsSortVal[WG_SIZE*ELEMENTS_PER_WORK_ITEM+16]; + __local u32 localHistogramToCarry[NUM_BUCKET]; + __local u32 localHistogram[NUM_BUCKET*2]; + + u32 gIdx = GET_GLOBAL_IDX; + u32 lIdx = GET_LOCAL_IDX; + u32 wgIdx = GET_GROUP_IDX; + u32 wgSize = GET_GROUP_SIZE; + + const int n = cb.m_n; + const int nWGs = cb.m_nWGs; + const int startBit = cb.m_startBit; + const int nBlocksPerWG = cb.m_nBlocksPerWG; + + if( lIdx < (NUM_BUCKET) ) + { + localHistogramToCarry[lIdx] = rHistogram[lIdx*nWGs + wgIdx]; + } + + GROUP_LDS_BARRIER; + + + const int blockSize = ELEMENTS_PER_WORK_ITEM*WG_SIZE; + + int nBlocks = n/blockSize - nBlocksPerWG*wgIdx; + + int addr = blockSize*nBlocksPerWG*wgIdx + ELEMENTS_PER_WORK_ITEM*lIdx; + + for(int iblock=0; iblock>startBit) & 0xf; + + { // create histogram + u32 setIdx = lIdx/16; + if( lIdx < NUM_BUCKET ) + { + localHistogram[lIdx] = 0; + } + ldsSortData[lIdx] = 0; + GROUP_LDS_BARRIER; + + for(int i=0; i0) + return; + + for (int c=0;c>startBit) & 0xf;//0xf = NUM_TABLES-1 + gDst[rHistogram[tableIdx*nWGs+wgIdx] + counter[tableIdx]] = gSrc[i]; + counter[tableIdx] ++; + } + } + } + } + +} + + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void SortAndScatterKernelSerial( __global const u32* restrict gSrc, __global const u32* rHistogram, __global u32* restrict gDst, int4 cb ) +{ + + u32 gIdx = GET_GLOBAL_IDX; + u32 realLocalIdx = GET_LOCAL_IDX; + u32 wgIdx = GET_GROUP_IDX; + u32 wgSize = GET_GROUP_SIZE; + const int startBit = cb.m_startBit; + const int n = cb.m_n; + const int nWGs = cb.m_nWGs; + const int nBlocksPerWG = cb.m_nBlocksPerWG; + + int counter[NUM_BUCKET]; + + if (realLocalIdx>0) + return; + + for (int c=0;c>startBit) & 0xf;//0xf = NUM_TABLES-1 + gDst[rHistogram[tableIdx*nWGs+wgIdx] + counter[tableIdx]] = gSrc[i]; + counter[tableIdx] ++; + } + } + } + } + +} \ No newline at end of file diff --git a/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/RadixSort32KernelsCL.h b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/RadixSort32KernelsCL.h new file mode 100644 index 000000000000..8876c16aa641 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/ParallelPrimitives/kernels/RadixSort32KernelsCL.h @@ -0,0 +1,910 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* radixSort32KernelsCL= \ +"/*\n" +"Bullet Continuous Collision Detection and Physics Library\n" +"Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org\n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose, \n" +"including commercial applications, and to alter it and redistribute it freely, \n" +"subject to the following restrictions:\n" +"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.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"//Author Takahiro Harada\n" +"//#pragma OPENCL EXTENSION cl_amd_printf : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" +"typedef unsigned int u32;\n" +"#define GET_GROUP_IDX get_group_id(0)\n" +"#define GET_LOCAL_IDX get_local_id(0)\n" +"#define GET_GLOBAL_IDX get_global_id(0)\n" +"#define GET_GROUP_SIZE get_local_size(0)\n" +"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" +"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" +"#define AtomInc(x) atom_inc(&(x))\n" +"#define AtomInc1(x, out) out = atom_inc(&(x))\n" +"#define AtomAdd(x, value) atom_add(&(x), value)\n" +"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" +"#define make_uint4 (uint4)\n" +"#define make_uint2 (uint2)\n" +"#define make_int2 (int2)\n" +"#define WG_SIZE 64\n" +"#define ELEMENTS_PER_WORK_ITEM (256/WG_SIZE)\n" +"#define BITS_PER_PASS 4\n" +"#define NUM_BUCKET (1< 64 )\n" +" {\n" +" sorterSharedMemory[idx] += sorterSharedMemory[idx-64];\n" +" GROUP_MEM_FENCE;\n" +" }\n" +" sorterSharedMemory[idx-1] += sorterSharedMemory[idx-2];\n" +" GROUP_MEM_FENCE;\n" +" }\n" +"#else\n" +" if( lIdx < 64 )\n" +" {\n" +" sorterSharedMemory[idx] += sorterSharedMemory[idx-1];\n" +" GROUP_MEM_FENCE;\n" +" sorterSharedMemory[idx] += sorterSharedMemory[idx-2]; \n" +" GROUP_MEM_FENCE;\n" +" sorterSharedMemory[idx] += sorterSharedMemory[idx-4];\n" +" GROUP_MEM_FENCE;\n" +" sorterSharedMemory[idx] += sorterSharedMemory[idx-8];\n" +" GROUP_MEM_FENCE;\n" +" sorterSharedMemory[idx] += sorterSharedMemory[idx-16];\n" +" GROUP_MEM_FENCE;\n" +" sorterSharedMemory[idx] += sorterSharedMemory[idx-32];\n" +" GROUP_MEM_FENCE;\n" +" if( wgSize > 64 )\n" +" {\n" +" sorterSharedMemory[idx] += sorterSharedMemory[idx-64];\n" +" GROUP_MEM_FENCE;\n" +" }\n" +" sorterSharedMemory[idx-1] += sorterSharedMemory[idx-2];\n" +" GROUP_MEM_FENCE;\n" +" }\n" +"#endif\n" +" }\n" +" GROUP_LDS_BARRIER;\n" +" *totalSum = sorterSharedMemory[wgSize*2-1];\n" +" u32 addValue = sorterSharedMemory[lIdx+wgSize-1];\n" +" return addValue;\n" +"}\n" +"//__attribute__((reqd_work_group_size(128,1,1)))\n" +"uint4 localPrefixSum128V( uint4 pData, uint lIdx, uint* totalSum, __local u32* sorterSharedMemory )\n" +"{\n" +" u32 s4 = prefixScanVectorEx( &pData );\n" +" u32 rank = localPrefixSum( s4, lIdx, totalSum, sorterSharedMemory, 128 );\n" +" return pData + make_uint4( rank, rank, rank, rank );\n" +"}\n" +"//__attribute__((reqd_work_group_size(64,1,1)))\n" +"uint4 localPrefixSum64V( uint4 pData, uint lIdx, uint* totalSum, __local u32* sorterSharedMemory )\n" +"{\n" +" u32 s4 = prefixScanVectorEx( &pData );\n" +" u32 rank = localPrefixSum( s4, lIdx, totalSum, sorterSharedMemory, 64 );\n" +" return pData + make_uint4( rank, rank, rank, rank );\n" +"}\n" +"u32 unpack4Key( u32 key, int keyIdx ){ return (key>>(keyIdx*8)) & 0xff;}\n" +"u32 bit8Scan(u32 v)\n" +"{\n" +" return (v<<8) + (v<<16) + (v<<24);\n" +"}\n" +"//===\n" +"#define MY_HISTOGRAM(idx) localHistogramMat[(idx)*WG_SIZE+lIdx]\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void StreamCountKernel( __global u32* gSrc, __global u32* histogramOut, int4 cb )\n" +"{\n" +" __local u32 localHistogramMat[NUM_BUCKET*WG_SIZE];\n" +" u32 gIdx = GET_GLOBAL_IDX;\n" +" u32 lIdx = GET_LOCAL_IDX;\n" +" u32 wgIdx = GET_GROUP_IDX;\n" +" u32 wgSize = GET_GROUP_SIZE;\n" +" const int startBit = cb.m_startBit;\n" +" const int n = cb.m_n;\n" +" const int nWGs = cb.m_nWGs;\n" +" const int nBlocksPerWG = cb.m_nBlocksPerWG;\n" +" for(int i=0; i>startBit) & 0xf;\n" +"#if defined(NV_GPU)\n" +" MY_HISTOGRAM( localKey )++;\n" +"#else\n" +" AtomInc( MY_HISTOGRAM( localKey ) );\n" +"#endif\n" +" }\n" +" }\n" +" }\n" +" GROUP_LDS_BARRIER;\n" +" \n" +" if( lIdx < NUM_BUCKET )\n" +" {\n" +" u32 sum = 0;\n" +" for(int i=0; i>startBit) & 0xf;\n" +"#if defined(NV_GPU)\n" +" MY_HISTOGRAM( localKey )++;\n" +"#else\n" +" AtomInc( MY_HISTOGRAM( localKey ) );\n" +"#endif\n" +" }\n" +" }\n" +" }\n" +" GROUP_LDS_BARRIER;\n" +" \n" +" if( lIdx < NUM_BUCKET )\n" +" {\n" +" u32 sum = 0;\n" +" for(int i=0; i>startBit) & mask, (sortData[1]>>startBit) & mask, (sortData[2]>>startBit) & mask, (sortData[3]>>startBit) & mask );\n" +" uint4 prefixSum = SELECT_UINT4( make_uint4(1,1,1,1), make_uint4(0,0,0,0), cmpResult != make_uint4(0,0,0,0) );\n" +" u32 total;\n" +" prefixSum = localPrefixSum64V( prefixSum, lIdx, &total, ldsSortData );\n" +" {\n" +" uint4 localAddr = make_uint4(lIdx*4+0,lIdx*4+1,lIdx*4+2,lIdx*4+3);\n" +" uint4 dstAddr = localAddr - prefixSum + make_uint4( total, total, total, total );\n" +" dstAddr = SELECT_UINT4( prefixSum, dstAddr, cmpResult != make_uint4(0, 0, 0, 0) );\n" +" GROUP_LDS_BARRIER;\n" +" ldsSortData[dstAddr.x] = sortData[0];\n" +" ldsSortData[dstAddr.y] = sortData[1];\n" +" ldsSortData[dstAddr.z] = sortData[2];\n" +" ldsSortData[dstAddr.w] = sortData[3];\n" +" GROUP_LDS_BARRIER;\n" +" sortData[0] = ldsSortData[localAddr.x];\n" +" sortData[1] = ldsSortData[localAddr.y];\n" +" sortData[2] = ldsSortData[localAddr.z];\n" +" sortData[3] = ldsSortData[localAddr.w];\n" +" GROUP_LDS_BARRIER;\n" +" }\n" +" }\n" +"}\n" +"// 2 scan, 2 exchange\n" +"void sort4Bits1(u32 sortData[4], int startBit, int lIdx, __local u32* ldsSortData)\n" +"{\n" +" for(uint ibit=0; ibit>(startBit+ibit)) & 0x3, \n" +" (sortData[1]>>(startBit+ibit)) & 0x3, \n" +" (sortData[2]>>(startBit+ibit)) & 0x3, \n" +" (sortData[3]>>(startBit+ibit)) & 0x3);\n" +" u32 key4;\n" +" u32 sKeyPacked[4] = { 0, 0, 0, 0 };\n" +" {\n" +" sKeyPacked[0] |= 1<<(8*b.x);\n" +" sKeyPacked[1] |= 1<<(8*b.y);\n" +" sKeyPacked[2] |= 1<<(8*b.z);\n" +" sKeyPacked[3] |= 1<<(8*b.w);\n" +" key4 = sKeyPacked[0] + sKeyPacked[1] + sKeyPacked[2] + sKeyPacked[3];\n" +" }\n" +" u32 rankPacked;\n" +" u32 sumPacked;\n" +" {\n" +" rankPacked = localPrefixSum( key4, lIdx, &sumPacked, ldsSortData, WG_SIZE );\n" +" }\n" +" GROUP_LDS_BARRIER;\n" +" u32 newOffset[4] = { 0,0,0,0 };\n" +" {\n" +" u32 sumScanned = bit8Scan( sumPacked );\n" +" u32 scannedKeys[4];\n" +" scannedKeys[0] = 1<<(8*b.x);\n" +" scannedKeys[1] = 1<<(8*b.y);\n" +" scannedKeys[2] = 1<<(8*b.z);\n" +" scannedKeys[3] = 1<<(8*b.w);\n" +" { // 4 scans at once\n" +" u32 sum4 = 0;\n" +" for(int ie=0; ie<4; ie++)\n" +" {\n" +" u32 tmp = scannedKeys[ie];\n" +" scannedKeys[ie] = sum4;\n" +" sum4 += tmp;\n" +" }\n" +" }\n" +" {\n" +" u32 sumPlusRank = sumScanned + rankPacked;\n" +" { u32 ie = b.x;\n" +" scannedKeys[0] += sumPlusRank;\n" +" newOffset[0] = unpack4Key( scannedKeys[0], ie );\n" +" }\n" +" { u32 ie = b.y;\n" +" scannedKeys[1] += sumPlusRank;\n" +" newOffset[1] = unpack4Key( scannedKeys[1], ie );\n" +" }\n" +" { u32 ie = b.z;\n" +" scannedKeys[2] += sumPlusRank;\n" +" newOffset[2] = unpack4Key( scannedKeys[2], ie );\n" +" }\n" +" { u32 ie = b.w;\n" +" scannedKeys[3] += sumPlusRank;\n" +" newOffset[3] = unpack4Key( scannedKeys[3], ie );\n" +" }\n" +" }\n" +" }\n" +" GROUP_LDS_BARRIER;\n" +" {\n" +" ldsSortData[newOffset[0]] = sortData[0];\n" +" ldsSortData[newOffset[1]] = sortData[1];\n" +" ldsSortData[newOffset[2]] = sortData[2];\n" +" ldsSortData[newOffset[3]] = sortData[3];\n" +" GROUP_LDS_BARRIER;\n" +" u32 dstAddr = 4*lIdx;\n" +" sortData[0] = ldsSortData[dstAddr+0];\n" +" sortData[1] = ldsSortData[dstAddr+1];\n" +" sortData[2] = ldsSortData[dstAddr+2];\n" +" sortData[3] = ldsSortData[dstAddr+3];\n" +" GROUP_LDS_BARRIER;\n" +" }\n" +" }\n" +"}\n" +"#define SET_HISTOGRAM(setIdx, key) ldsSortData[(setIdx)*NUM_BUCKET+key]\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void SortAndScatterKernel( __global const u32* restrict gSrc, __global const u32* rHistogram, __global u32* restrict gDst, int4 cb )\n" +"{\n" +" __local u32 ldsSortData[WG_SIZE*ELEMENTS_PER_WORK_ITEM+16];\n" +" __local u32 localHistogramToCarry[NUM_BUCKET];\n" +" __local u32 localHistogram[NUM_BUCKET*2];\n" +" u32 gIdx = GET_GLOBAL_IDX;\n" +" u32 lIdx = GET_LOCAL_IDX;\n" +" u32 wgIdx = GET_GROUP_IDX;\n" +" u32 wgSize = GET_GROUP_SIZE;\n" +" const int n = cb.m_n;\n" +" const int nWGs = cb.m_nWGs;\n" +" const int startBit = cb.m_startBit;\n" +" const int nBlocksPerWG = cb.m_nBlocksPerWG;\n" +" if( lIdx < (NUM_BUCKET) )\n" +" {\n" +" localHistogramToCarry[lIdx] = rHistogram[lIdx*nWGs + wgIdx];\n" +" }\n" +" GROUP_LDS_BARRIER;\n" +" const int blockSize = ELEMENTS_PER_WORK_ITEM*WG_SIZE;\n" +" int nBlocks = n/blockSize - nBlocksPerWG*wgIdx;\n" +" int addr = blockSize*nBlocksPerWG*wgIdx + ELEMENTS_PER_WORK_ITEM*lIdx;\n" +" for(int iblock=0; iblock>startBit) & 0xf;\n" +" { // create histogram\n" +" u32 setIdx = lIdx/16;\n" +" if( lIdx < NUM_BUCKET )\n" +" {\n" +" localHistogram[lIdx] = 0;\n" +" }\n" +" ldsSortData[lIdx] = 0;\n" +" GROUP_LDS_BARRIER;\n" +" for(int i=0; i>(startBit+ibit)) & 0x3, \n" +" (sortData[1]>>(startBit+ibit)) & 0x3, \n" +" (sortData[2]>>(startBit+ibit)) & 0x3, \n" +" (sortData[3]>>(startBit+ibit)) & 0x3);\n" +" u32 key4;\n" +" u32 sKeyPacked[4] = { 0, 0, 0, 0 };\n" +" {\n" +" sKeyPacked[0] |= 1<<(8*b.x);\n" +" sKeyPacked[1] |= 1<<(8*b.y);\n" +" sKeyPacked[2] |= 1<<(8*b.z);\n" +" sKeyPacked[3] |= 1<<(8*b.w);\n" +" key4 = sKeyPacked[0] + sKeyPacked[1] + sKeyPacked[2] + sKeyPacked[3];\n" +" }\n" +" u32 rankPacked;\n" +" u32 sumPacked;\n" +" {\n" +" rankPacked = localPrefixSum( key4, lIdx, &sumPacked, ldsSortData, WG_SIZE );\n" +" }\n" +" GROUP_LDS_BARRIER;\n" +" u32 newOffset[4] = { 0,0,0,0 };\n" +" {\n" +" u32 sumScanned = bit8Scan( sumPacked );\n" +" u32 scannedKeys[4];\n" +" scannedKeys[0] = 1<<(8*b.x);\n" +" scannedKeys[1] = 1<<(8*b.y);\n" +" scannedKeys[2] = 1<<(8*b.z);\n" +" scannedKeys[3] = 1<<(8*b.w);\n" +" { // 4 scans at once\n" +" u32 sum4 = 0;\n" +" for(int ie=0; ie<4; ie++)\n" +" {\n" +" u32 tmp = scannedKeys[ie];\n" +" scannedKeys[ie] = sum4;\n" +" sum4 += tmp;\n" +" }\n" +" }\n" +" {\n" +" u32 sumPlusRank = sumScanned + rankPacked;\n" +" { u32 ie = b.x;\n" +" scannedKeys[0] += sumPlusRank;\n" +" newOffset[0] = unpack4Key( scannedKeys[0], ie );\n" +" }\n" +" { u32 ie = b.y;\n" +" scannedKeys[1] += sumPlusRank;\n" +" newOffset[1] = unpack4Key( scannedKeys[1], ie );\n" +" }\n" +" { u32 ie = b.z;\n" +" scannedKeys[2] += sumPlusRank;\n" +" newOffset[2] = unpack4Key( scannedKeys[2], ie );\n" +" }\n" +" { u32 ie = b.w;\n" +" scannedKeys[3] += sumPlusRank;\n" +" newOffset[3] = unpack4Key( scannedKeys[3], ie );\n" +" }\n" +" }\n" +" }\n" +" GROUP_LDS_BARRIER;\n" +" {\n" +" ldsSortData[newOffset[0]] = sortData[0];\n" +" ldsSortData[newOffset[1]] = sortData[1];\n" +" ldsSortData[newOffset[2]] = sortData[2];\n" +" ldsSortData[newOffset[3]] = sortData[3];\n" +" ldsSortVal[newOffset[0]] = sortVal[0];\n" +" ldsSortVal[newOffset[1]] = sortVal[1];\n" +" ldsSortVal[newOffset[2]] = sortVal[2];\n" +" ldsSortVal[newOffset[3]] = sortVal[3];\n" +" GROUP_LDS_BARRIER;\n" +" u32 dstAddr = 4*lIdx;\n" +" sortData[0] = ldsSortData[dstAddr+0];\n" +" sortData[1] = ldsSortData[dstAddr+1];\n" +" sortData[2] = ldsSortData[dstAddr+2];\n" +" sortData[3] = ldsSortData[dstAddr+3];\n" +" sortVal[0] = ldsSortVal[dstAddr+0];\n" +" sortVal[1] = ldsSortVal[dstAddr+1];\n" +" sortVal[2] = ldsSortVal[dstAddr+2];\n" +" sortVal[3] = ldsSortVal[dstAddr+3];\n" +" GROUP_LDS_BARRIER;\n" +" }\n" +" }\n" +"}\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void SortAndScatterSortDataKernel( __global const SortDataCL* restrict gSrc, __global const u32* rHistogram, __global SortDataCL* restrict gDst, int4 cb)\n" +"{\n" +" __local int ldsSortData[WG_SIZE*ELEMENTS_PER_WORK_ITEM+16];\n" +" __local int ldsSortVal[WG_SIZE*ELEMENTS_PER_WORK_ITEM+16];\n" +" __local u32 localHistogramToCarry[NUM_BUCKET];\n" +" __local u32 localHistogram[NUM_BUCKET*2];\n" +" u32 gIdx = GET_GLOBAL_IDX;\n" +" u32 lIdx = GET_LOCAL_IDX;\n" +" u32 wgIdx = GET_GROUP_IDX;\n" +" u32 wgSize = GET_GROUP_SIZE;\n" +" const int n = cb.m_n;\n" +" const int nWGs = cb.m_nWGs;\n" +" const int startBit = cb.m_startBit;\n" +" const int nBlocksPerWG = cb.m_nBlocksPerWG;\n" +" if( lIdx < (NUM_BUCKET) )\n" +" {\n" +" localHistogramToCarry[lIdx] = rHistogram[lIdx*nWGs + wgIdx];\n" +" }\n" +" GROUP_LDS_BARRIER;\n" +" \n" +" const int blockSize = ELEMENTS_PER_WORK_ITEM*WG_SIZE;\n" +" int nBlocks = n/blockSize - nBlocksPerWG*wgIdx;\n" +" int addr = blockSize*nBlocksPerWG*wgIdx + ELEMENTS_PER_WORK_ITEM*lIdx;\n" +" for(int iblock=0; iblock>startBit) & 0xf;\n" +" { // create histogram\n" +" u32 setIdx = lIdx/16;\n" +" if( lIdx < NUM_BUCKET )\n" +" {\n" +" localHistogram[lIdx] = 0;\n" +" }\n" +" ldsSortData[lIdx] = 0;\n" +" GROUP_LDS_BARRIER;\n" +" for(int i=0; i0)\n" +" return;\n" +" \n" +" for (int c=0;c>startBit) & 0xf;//0xf = NUM_TABLES-1\n" +" gDst[rHistogram[tableIdx*nWGs+wgIdx] + counter[tableIdx]] = gSrc[i];\n" +" counter[tableIdx] ++;\n" +" }\n" +" }\n" +" }\n" +" }\n" +" \n" +"}\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void SortAndScatterKernelSerial( __global const u32* restrict gSrc, __global const u32* rHistogram, __global u32* restrict gDst, int4 cb )\n" +"{\n" +" \n" +" u32 gIdx = GET_GLOBAL_IDX;\n" +" u32 realLocalIdx = GET_LOCAL_IDX;\n" +" u32 wgIdx = GET_GROUP_IDX;\n" +" u32 wgSize = GET_GROUP_SIZE;\n" +" const int startBit = cb.m_startBit;\n" +" const int n = cb.m_n;\n" +" const int nWGs = cb.m_nWGs;\n" +" const int nBlocksPerWG = cb.m_nBlocksPerWG;\n" +" int counter[NUM_BUCKET];\n" +" \n" +" if (realLocalIdx>0)\n" +" return;\n" +" \n" +" for (int c=0;c>startBit) & 0xf;//0xf = NUM_TABLES-1\n" +" gDst[rHistogram[tableIdx*nWGs+wgIdx] + counter[tableIdx]] = gSrc[i];\n" +" counter[tableIdx] ++;\n" +" }\n" +" }\n" +" }\n" +" }\n" +" \n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp b/extern/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp new file mode 100644 index 000000000000..161e304f090e --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp @@ -0,0 +1,391 @@ + +#include "b3GpuRaycast.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h" + + +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3FillCL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h" +#include "Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h" +#include "Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.h" + +#include "Bullet3OpenCL/Raycast/kernels/rayCastKernels.h" + + +#define B3_RAYCAST_PATH "src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl" + + + +struct b3GpuRaycastInternalData +{ + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_q; + cl_kernel m_raytraceKernel; + cl_kernel m_raytracePairsKernel; + cl_kernel m_findRayRigidPairIndexRanges; + + b3GpuParallelLinearBvh* m_plbvh; + b3RadixSort32CL* m_radixSorter; + b3FillCL* m_fill; + + //1 element per ray + b3OpenCLArray* m_gpuRays; + b3OpenCLArray* m_gpuHitResults; + b3OpenCLArray* m_firstRayRigidPairIndexPerRay; + b3OpenCLArray* m_numRayRigidPairsPerRay; + + //1 element per (ray index, rigid index) pair, where the ray intersects with the rigid's AABB + b3OpenCLArray* m_gpuNumRayRigidPairs; + b3OpenCLArray* m_gpuRayRigidPairs; //x == ray index, y == rigid index + + int m_test; +}; + +b3GpuRaycast::b3GpuRaycast(cl_context ctx,cl_device_id device, cl_command_queue q) +{ + m_data = new b3GpuRaycastInternalData; + m_data->m_context = ctx; + m_data->m_device = device; + m_data->m_q = q; + m_data->m_raytraceKernel = 0; + m_data->m_raytracePairsKernel = 0; + m_data->m_findRayRigidPairIndexRanges = 0; + + m_data->m_plbvh = new b3GpuParallelLinearBvh(ctx, device, q); + m_data->m_radixSorter = new b3RadixSort32CL(ctx, device, q); + m_data->m_fill = new b3FillCL(ctx, device, q); + + m_data->m_gpuRays = new b3OpenCLArray(ctx, q); + m_data->m_gpuHitResults = new b3OpenCLArray(ctx, q); + m_data->m_firstRayRigidPairIndexPerRay = new b3OpenCLArray(ctx, q); + m_data->m_numRayRigidPairsPerRay = new b3OpenCLArray(ctx, q); + m_data->m_gpuNumRayRigidPairs = new b3OpenCLArray(ctx, q); + m_data->m_gpuRayRigidPairs = new b3OpenCLArray(ctx, q); + + { + cl_int errNum=0; + cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_data->m_context,m_data->m_device,rayCastKernelCL,&errNum,"",B3_RAYCAST_PATH); + b3Assert(errNum==CL_SUCCESS); + m_data->m_raytraceKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,rayCastKernelCL, "rayCastKernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + m_data->m_raytracePairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,rayCastKernelCL, "rayCastPairsKernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + m_data->m_findRayRigidPairIndexRanges = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,rayCastKernelCL, "findRayRigidPairIndexRanges",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + clReleaseProgram(prog); + } + + +} + +b3GpuRaycast::~b3GpuRaycast() +{ + clReleaseKernel(m_data->m_raytraceKernel); + clReleaseKernel(m_data->m_raytracePairsKernel); + clReleaseKernel(m_data->m_findRayRigidPairIndexRanges); + + delete m_data->m_plbvh; + delete m_data->m_radixSorter; + delete m_data->m_fill; + + delete m_data->m_gpuRays; + delete m_data->m_gpuHitResults; + delete m_data->m_firstRayRigidPairIndexPerRay; + delete m_data->m_numRayRigidPairsPerRay; + delete m_data->m_gpuNumRayRigidPairs; + delete m_data->m_gpuRayRigidPairs; + + delete m_data; +} + +bool sphere_intersect(const b3Vector3& spherePos, b3Scalar radius, const b3Vector3& rayFrom, const b3Vector3& rayTo, float& hitFraction) +{ + b3Vector3 rs = rayFrom - spherePos; + b3Vector3 rayDir = rayTo-rayFrom; + + float A = b3Dot(rayDir,rayDir); + float B = b3Dot(rs, rayDir); + float C = b3Dot(rs, rs) - (radius * radius); + + float D = B * B - A*C; + + if (D > 0.0) + { + float t = (-B - sqrt(D))/A; + + if ( (t >= 0.0f) && (t < hitFraction) ) + { + hitFraction = t; + return true; + } + } + return false; +} + +bool rayConvex(const b3Vector3& rayFromLocal, const b3Vector3& rayToLocal, const b3ConvexPolyhedronData& poly, + const b3AlignedObjectArray& faces, float& hitFraction, b3Vector3& hitNormal) +{ + float exitFraction = hitFraction; + float enterFraction = -0.1f; + b3Vector3 curHitNormal=b3MakeVector3(0,0,0); + for (int i=0;i= 0.f) + { + float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist); + if (exitFraction>fraction) + { + exitFraction = fraction; + } + } + } else + { + if (toPlaneDist<0.f) + { + float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist); + if (enterFraction <= fraction) + { + enterFraction = fraction; + curHitNormal = face.m_plane; + curHitNormal.w = 0.f; + } + } else + { + return false; + } + } + if (exitFraction <= enterFraction) + return false; + } + + if (enterFraction < 0.f) + return false; + + hitFraction = enterFraction; + hitNormal = curHitNormal; + return true; +} + +void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray& rays, b3AlignedObjectArray& hitResults, + int numBodies,const struct b3RigidBodyData* bodies, int numCollidables,const struct b3Collidable* collidables, const struct b3GpuNarrowPhaseInternalData* narrowphaseData) +{ + +// return castRays(rays,hitResults,numBodies,bodies,numCollidables,collidables); + + B3_PROFILE("castRaysHost"); + for (int r=0;rm_convexPolyhedra[shapeIndex]; + if (rayConvex(rayFromLocal, rayToLocal,poly,narrowphaseData->m_convexFaces, hitFraction, hitNormal)) + { + hitBodyIndex = b; + } + + + break; + } + default: + { + static bool once=true; + if (once) + { + once=false; + b3Warning("Raytest: unsupported shape type\n"); + } + } + } + } + if (hitBodyIndex>=0) + { + + hitResults[r].m_hitFraction = hitFraction; + hitResults[r].m_hitPoint.setInterpolate3(rays[r].m_from, rays[r].m_to,hitFraction); + hitResults[r].m_hitNormal = hitNormal; + hitResults[r].m_hitBody = hitBodyIndex; + } + + } +} +///todo: add some acceleration structure (AABBs, tree etc) +void b3GpuRaycast::castRays(const b3AlignedObjectArray& rays, b3AlignedObjectArray& hitResults, + int numBodies,const struct b3RigidBodyData* bodies, int numCollidables, const struct b3Collidable* collidables, + const struct b3GpuNarrowPhaseInternalData* narrowphaseData, class b3GpuBroadphaseInterface* broadphase) +{ + //castRaysHost(rays,hitResults,numBodies,bodies,numCollidables,collidables,narrowphaseData); + + B3_PROFILE("castRaysGPU"); + + { + B3_PROFILE("raycast copyFromHost"); + m_data->m_gpuRays->copyFromHost(rays); + m_data->m_gpuHitResults->copyFromHost(hitResults); + + } + + int numRays = hitResults.size(); + { + m_data->m_firstRayRigidPairIndexPerRay->resize(numRays); + m_data->m_numRayRigidPairsPerRay->resize(numRays); + + m_data->m_gpuNumRayRigidPairs->resize(1); + m_data->m_gpuRayRigidPairs->resize(numRays * 16); + } + + //run kernel + const bool USE_BRUTE_FORCE_RAYCAST = false; + if(USE_BRUTE_FORCE_RAYCAST) + { + B3_PROFILE("raycast launch1D"); + + b3LauncherCL launcher(m_data->m_q,m_data->m_raytraceKernel,"m_raytraceKernel"); + int numRays = rays.size(); + launcher.setConst(numRays); + + launcher.setBuffer(m_data->m_gpuRays->getBufferCL()); + launcher.setBuffer(m_data->m_gpuHitResults->getBufferCL()); + + launcher.setConst(numBodies); + launcher.setBuffer(narrowphaseData->m_bodyBufferGPU->getBufferCL()); + launcher.setBuffer(narrowphaseData->m_collidablesGPU->getBufferCL()); + launcher.setBuffer(narrowphaseData->m_convexFacesGPU->getBufferCL()); + launcher.setBuffer(narrowphaseData->m_convexPolyhedraGPU->getBufferCL()); + + launcher.launch1D(numRays); + clFinish(m_data->m_q); + } + else + { + m_data->m_plbvh->build( broadphase->getAllAabbsGPU(), broadphase->getSmallAabbIndicesGPU(), broadphase->getLargeAabbIndicesGPU() ); + + m_data->m_plbvh->testRaysAgainstBvhAabbs(*m_data->m_gpuRays, *m_data->m_gpuNumRayRigidPairs, *m_data->m_gpuRayRigidPairs); + + int numRayRigidPairs = -1; + m_data->m_gpuNumRayRigidPairs->copyToHostPointer(&numRayRigidPairs, 1); + if( numRayRigidPairs > m_data->m_gpuRayRigidPairs->size() ) + { + numRayRigidPairs = m_data->m_gpuRayRigidPairs->size(); + m_data->m_gpuNumRayRigidPairs->copyFromHostPointer(&numRayRigidPairs, 1); + } + + m_data->m_gpuRayRigidPairs->resize(numRayRigidPairs); //Radix sort needs b3OpenCLArray::size() to be correct + + //Sort ray-rigid pairs by ray index + { + B3_PROFILE("sort ray-rigid pairs"); + m_data->m_radixSorter->execute( *reinterpret_cast< b3OpenCLArray* >(m_data->m_gpuRayRigidPairs) ); + } + + //detect start,count of each ray pair + { + B3_PROFILE("detect ray-rigid pair index ranges"); + + { + B3_PROFILE("reset ray-rigid pair index ranges"); + + m_data->m_fill->execute(*m_data->m_firstRayRigidPairIndexPerRay, numRayRigidPairs, numRays); //atomic_min used to find first index + m_data->m_fill->execute(*m_data->m_numRayRigidPairsPerRay, 0, numRays); + clFinish(m_data->m_q); + } + + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_data->m_gpuRayRigidPairs->getBufferCL() ), + + b3BufferInfoCL( m_data->m_firstRayRigidPairIndexPerRay->getBufferCL() ), + b3BufferInfoCL( m_data->m_numRayRigidPairsPerRay->getBufferCL() ) + }; + + b3LauncherCL launcher(m_data->m_q, m_data->m_findRayRigidPairIndexRanges, "m_findRayRigidPairIndexRanges"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(numRayRigidPairs); + + launcher.launch1D(numRayRigidPairs); + clFinish(m_data->m_q); + } + + { + B3_PROFILE("ray-rigid intersection"); + + b3BufferInfoCL bufferInfo[] = + { + b3BufferInfoCL( m_data->m_gpuRays->getBufferCL() ), + b3BufferInfoCL( m_data->m_gpuHitResults->getBufferCL() ), + b3BufferInfoCL( m_data->m_firstRayRigidPairIndexPerRay->getBufferCL() ), + b3BufferInfoCL( m_data->m_numRayRigidPairsPerRay->getBufferCL() ), + + b3BufferInfoCL( narrowphaseData->m_bodyBufferGPU->getBufferCL() ), + b3BufferInfoCL( narrowphaseData->m_collidablesGPU->getBufferCL() ), + b3BufferInfoCL( narrowphaseData->m_convexFacesGPU->getBufferCL() ), + b3BufferInfoCL( narrowphaseData->m_convexPolyhedraGPU->getBufferCL() ), + + b3BufferInfoCL( m_data->m_gpuRayRigidPairs->getBufferCL() ) + }; + + b3LauncherCL launcher(m_data->m_q, m_data->m_raytracePairsKernel, "m_raytracePairsKernel"); + launcher.setBuffers( bufferInfo, sizeof(bufferInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst(numRays); + + launcher.launch1D(numRays); + clFinish(m_data->m_q); + } + } + + + + //copy results + { + B3_PROFILE("raycast copyToHost"); + m_data->m_gpuHitResults->copyToHost(hitResults); + } + +} \ No newline at end of file diff --git a/extern/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.h b/extern/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.h new file mode 100644 index 000000000000..3a5cf44b7956 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/Raycast/b3GpuRaycast.h @@ -0,0 +1,32 @@ +#ifndef B3_GPU_RAYCAST_H +#define B3_GPU_RAYCAST_H + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" + +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h" + + + +class b3GpuRaycast +{ +protected: + struct b3GpuRaycastInternalData* m_data; +public: + b3GpuRaycast(cl_context ctx,cl_device_id device, cl_command_queue q); + virtual ~b3GpuRaycast(); + + void castRaysHost(const b3AlignedObjectArray& raysIn, b3AlignedObjectArray& hitResults, + int numBodies, const struct b3RigidBodyData* bodies, int numCollidables, const struct b3Collidable* collidables, + const struct b3GpuNarrowPhaseInternalData* narrowphaseData); + + void castRays(const b3AlignedObjectArray& rays, b3AlignedObjectArray& hitResults, + int numBodies,const struct b3RigidBodyData* bodies, int numCollidables, const struct b3Collidable* collidables, + const struct b3GpuNarrowPhaseInternalData* narrowphaseData, class b3GpuBroadphaseInterface* broadphase); + + + +}; + +#endif //B3_GPU_RAYCAST_H diff --git a/extern/bullet/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl b/extern/bullet/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl new file mode 100644 index 000000000000..e72d96876b10 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl @@ -0,0 +1,439 @@ + +#define SHAPE_CONVEX_HULL 3 +#define SHAPE_PLANE 4 +#define SHAPE_CONCAVE_TRIMESH 5 +#define SHAPE_COMPOUND_OF_CONVEX_HULLS 6 +#define SHAPE_SPHERE 7 + + +typedef struct +{ + float4 m_from; + float4 m_to; +} b3RayInfo; + +typedef struct +{ + float m_hitFraction; + int m_hitResult0; + int m_hitResult1; + int m_hitResult2; + float4 m_hitPoint; + float4 m_hitNormal; +} b3RayHit; + +typedef struct +{ + float4 m_pos; + float4 m_quat; + float4 m_linVel; + float4 m_angVel; + + unsigned int m_collidableIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} Body; + +typedef struct Collidable +{ + union { + int m_numChildShapes; + int m_bvhIndex; + }; + float m_radius; + int m_shapeType; + int m_shapeIndex; +} Collidable; + + +typedef struct +{ + float4 m_localCenter; + float4 m_extents; + float4 mC; + float4 mE; + + float m_radius; + int m_faceOffset; + int m_numFaces; + int m_numVertices; + + int m_vertexOffset; + int m_uniqueEdgesOffset; + int m_numUniqueEdges; + int m_unused; + +} ConvexPolyhedronCL; + +typedef struct +{ + float4 m_plane; + int m_indexOffset; + int m_numIndices; +} b3GpuFace; + + + +/////////////////////////////////////// +// Quaternion +/////////////////////////////////////// + +typedef float4 Quaternion; + +__inline + Quaternion qtMul(Quaternion a, Quaternion b); + +__inline + Quaternion qtNormalize(Quaternion in); + + +__inline + Quaternion qtInvert(Quaternion q); + + +__inline + float dot3F4(float4 a, float4 b) +{ + float4 a1 = (float4)(a.xyz,0.f); + float4 b1 = (float4)(b.xyz,0.f); + return dot(a1, b1); +} + + +__inline + Quaternion qtMul(Quaternion a, Quaternion b) +{ + Quaternion ans; + ans = cross( a, b ); + ans += a.w*b+b.w*a; + // ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - dot3F4(a, b); + return ans; +} + +__inline + Quaternion qtNormalize(Quaternion in) +{ + return fast_normalize(in); + // in /= length( in ); + // return in; +} +__inline + float4 qtRotate(Quaternion q, float4 vec) +{ + Quaternion qInv = qtInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = qtMul(q,vcpy); + out = qtMul(out,qInv); + return out; +} + +__inline + Quaternion qtInvert(Quaternion q) +{ + return (Quaternion)(-q.xyz, q.w); +} + +__inline + float4 qtInvRotate(const Quaternion q, float4 vec) +{ + return qtRotate( qtInvert( q ), vec ); +} + + + +void trInverse(float4 translationIn, Quaternion orientationIn, + float4* translationOut, Quaternion* orientationOut) +{ + *orientationOut = qtInvert(orientationIn); + *translationOut = qtRotate(*orientationOut, -translationIn); +} + + + + + +bool rayConvex(float4 rayFromLocal, float4 rayToLocal, int numFaces, int faceOffset, + __global const b3GpuFace* faces, float* hitFraction, float4* hitNormal) +{ + rayFromLocal.w = 0.f; + rayToLocal.w = 0.f; + bool result = true; + + float exitFraction = hitFraction[0]; + float enterFraction = -0.3f; + float4 curHitNormal = (float4)(0,0,0,0); + for (int i=0;i= 0.f) + { + float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist); + if (exitFraction>fraction) + { + exitFraction = fraction; + } + } + } else + { + if (toPlaneDist<0.f) + { + float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist); + if (enterFraction <= fraction) + { + enterFraction = fraction; + curHitNormal = face.m_plane; + curHitNormal.w = 0.f; + } + } else + { + result = false; + } + } + if (exitFraction <= enterFraction) + result = false; + } + + if (enterFraction < 0.f) + { + result = false; + } + + if (result) + { + hitFraction[0] = enterFraction; + hitNormal[0] = curHitNormal; + } + return result; +} + + + + + + +bool sphere_intersect(float4 spherePos, float radius, float4 rayFrom, float4 rayTo, float* hitFraction) +{ + float4 rs = rayFrom - spherePos; + rs.w = 0.f; + float4 rayDir = rayTo-rayFrom; + rayDir.w = 0.f; + float A = dot(rayDir,rayDir); + float B = dot(rs, rayDir); + float C = dot(rs, rs) - (radius * radius); + + float D = B * B - A*C; + + if (D > 0.0f) + { + float t = (-B - sqrt(D))/A; + + if ( (t >= 0.0f) && (t < (*hitFraction)) ) + { + *hitFraction = t; + return true; + } + } + return false; +} + +float4 setInterpolate3(float4 from, float4 to, float t) +{ + float s = 1.0f - t; + float4 result; + result = s * from + t * to; + result.w = 0.f; + return result; +} + +__kernel void rayCastKernel( + int numRays, + const __global b3RayInfo* rays, + __global b3RayHit* hitResults, + const int numBodies, + __global Body* bodies, + __global Collidable* collidables, + __global const b3GpuFace* faces, + __global const ConvexPolyhedronCL* convexShapes ) +{ + + int i = get_global_id(0); + if (i>=numRays) + return; + + hitResults[i].m_hitFraction = 1.f; + + float4 rayFrom = rays[i].m_from; + float4 rayTo = rays[i].m_to; + float hitFraction = 1.f; + float4 hitPoint; + float4 hitNormal; + int hitBodyIndex= -1; + + int cachedCollidableIndex = -1; + Collidable cachedCollidable; + + for (int b=0;b=0) + { + hitPoint = setInterpolate3(rayFrom, rayTo,hitFraction); + hitResults[i].m_hitFraction = hitFraction; + hitResults[i].m_hitPoint = hitPoint; + hitResults[i].m_hitNormal = normalize(hitNormal); + hitResults[i].m_hitResult0 = hitBodyIndex; + } + +} + + +__kernel void findRayRigidPairIndexRanges(__global int2* rayRigidPairs, + __global int* out_firstRayRigidPairIndexPerRay, + __global int* out_numRayRigidPairsPerRay, + int numRayRigidPairs) +{ + int rayRigidPairIndex = get_global_id(0); + if (rayRigidPairIndex >= numRayRigidPairs) return; + + int rayIndex = rayRigidPairs[rayRigidPairIndex].x; + + atomic_min(&out_firstRayRigidPairIndexPerRay[rayIndex], rayRigidPairIndex); + atomic_inc(&out_numRayRigidPairsPerRay[rayIndex]); +} + +__kernel void rayCastPairsKernel(const __global b3RayInfo* rays, + __global b3RayHit* hitResults, + __global int* firstRayRigidPairIndexPerRay, + __global int* numRayRigidPairsPerRay, + + __global Body* bodies, + __global Collidable* collidables, + __global const b3GpuFace* faces, + __global const ConvexPolyhedronCL* convexShapes, + + __global int2* rayRigidPairs, + int numRays) +{ + int i = get_global_id(0); + if (i >= numRays) return; + + float4 rayFrom = rays[i].m_from; + float4 rayTo = rays[i].m_to; + + hitResults[i].m_hitFraction = 1.f; + + float hitFraction = 1.f; + float4 hitPoint; + float4 hitNormal; + int hitBodyIndex = -1; + + // + for(int pair = 0; pair < numRayRigidPairsPerRay[i]; ++pair) + { + int rayRigidPairIndex = pair + firstRayRigidPairIndexPerRay[i]; + int b = rayRigidPairs[rayRigidPairIndex].y; + + if (hitResults[i].m_hitResult2 == b) continue; + + Body body = bodies[b]; + Collidable rigidCollidable = collidables[body.m_collidableIdx]; + + float4 pos = body.m_pos; + float4 orn = body.m_quat; + + if (rigidCollidable.m_shapeType == SHAPE_CONVEX_HULL) + { + float4 invPos = (float4)(0,0,0,0); + float4 invOrn = (float4)(0,0,0,0); + float4 rayFromLocal = (float4)(0,0,0,0); + float4 rayToLocal = (float4)(0,0,0,0); + invOrn = qtInvert(orn); + invPos = qtRotate(invOrn, -pos); + rayFromLocal = qtRotate( invOrn, rayFrom ) + invPos; + rayToLocal = qtRotate( invOrn, rayTo) + invPos; + rayFromLocal.w = 0.f; + rayToLocal.w = 0.f; + int numFaces = convexShapes[rigidCollidable.m_shapeIndex].m_numFaces; + int faceOffset = convexShapes[rigidCollidable.m_shapeIndex].m_faceOffset; + + if (numFaces && rayConvex(rayFromLocal, rayToLocal, numFaces, faceOffset,faces, &hitFraction, &hitNormal)) + { + hitBodyIndex = b; + hitPoint = setInterpolate3(rayFrom, rayTo, hitFraction); + } + } + + if (rigidCollidable.m_shapeType == SHAPE_SPHERE) + { + float radius = rigidCollidable.m_radius; + + if (sphere_intersect(pos, radius, rayFrom, rayTo, &hitFraction)) + { + hitBodyIndex = b; + hitPoint = setInterpolate3(rayFrom, rayTo, hitFraction); + hitNormal = (float4) (hitPoint - bodies[b].m_pos); + } + } + } + + if (hitBodyIndex >= 0) + { + hitResults[i].m_hitFraction = hitFraction; + hitResults[i].m_hitPoint = hitPoint; + hitResults[i].m_hitNormal = normalize(hitNormal); + hitResults[i].m_hitResult0 = hitBodyIndex; + } + +} diff --git a/extern/bullet/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h b/extern/bullet/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h new file mode 100644 index 000000000000..6257909a4d6b --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h @@ -0,0 +1,381 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* rayCastKernelCL= \ +"#define SHAPE_CONVEX_HULL 3\n" +"#define SHAPE_PLANE 4\n" +"#define SHAPE_CONCAVE_TRIMESH 5\n" +"#define SHAPE_COMPOUND_OF_CONVEX_HULLS 6\n" +"#define SHAPE_SPHERE 7\n" +"typedef struct\n" +"{\n" +" float4 m_from;\n" +" float4 m_to;\n" +"} b3RayInfo;\n" +"typedef struct\n" +"{\n" +" float m_hitFraction;\n" +" int m_hitResult0;\n" +" int m_hitResult1;\n" +" int m_hitResult2;\n" +" float4 m_hitPoint;\n" +" float4 m_hitNormal;\n" +"} b3RayHit;\n" +"typedef struct\n" +"{\n" +" float4 m_pos;\n" +" float4 m_quat;\n" +" float4 m_linVel;\n" +" float4 m_angVel;\n" +" unsigned int m_collidableIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"} Body;\n" +"typedef struct Collidable\n" +"{\n" +" union {\n" +" int m_numChildShapes;\n" +" int m_bvhIndex;\n" +" };\n" +" float m_radius;\n" +" int m_shapeType;\n" +" int m_shapeIndex;\n" +"} Collidable;\n" +"typedef struct \n" +"{\n" +" float4 m_localCenter;\n" +" float4 m_extents;\n" +" float4 mC;\n" +" float4 mE;\n" +" float m_radius;\n" +" int m_faceOffset;\n" +" int m_numFaces;\n" +" int m_numVertices;\n" +" int m_vertexOffset;\n" +" int m_uniqueEdgesOffset;\n" +" int m_numUniqueEdges;\n" +" int m_unused;\n" +"} ConvexPolyhedronCL;\n" +"typedef struct\n" +"{\n" +" float4 m_plane;\n" +" int m_indexOffset;\n" +" int m_numIndices;\n" +"} b3GpuFace;\n" +"///////////////////////////////////////\n" +"// Quaternion\n" +"///////////////////////////////////////\n" +"typedef float4 Quaternion;\n" +"__inline\n" +" Quaternion qtMul(Quaternion a, Quaternion b);\n" +"__inline\n" +" Quaternion qtNormalize(Quaternion in);\n" +"__inline\n" +" Quaternion qtInvert(Quaternion q);\n" +"__inline\n" +" float dot3F4(float4 a, float4 b)\n" +"{\n" +" float4 a1 = (float4)(a.xyz,0.f);\n" +" float4 b1 = (float4)(b.xyz,0.f);\n" +" return dot(a1, b1);\n" +"}\n" +"__inline\n" +" Quaternion qtMul(Quaternion a, Quaternion b)\n" +"{\n" +" Quaternion ans;\n" +" ans = cross( a, b );\n" +" ans += a.w*b+b.w*a;\n" +" // ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"__inline\n" +" Quaternion qtNormalize(Quaternion in)\n" +"{\n" +" return fast_normalize(in);\n" +" // in /= length( in );\n" +" // return in;\n" +"}\n" +"__inline\n" +" float4 qtRotate(Quaternion q, float4 vec)\n" +"{\n" +" Quaternion qInv = qtInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = qtMul(q,vcpy);\n" +" out = qtMul(out,qInv);\n" +" return out;\n" +"}\n" +"__inline\n" +" Quaternion qtInvert(Quaternion q)\n" +"{\n" +" return (Quaternion)(-q.xyz, q.w);\n" +"}\n" +"__inline\n" +" float4 qtInvRotate(const Quaternion q, float4 vec)\n" +"{\n" +" return qtRotate( qtInvert( q ), vec );\n" +"}\n" +"void trInverse(float4 translationIn, Quaternion orientationIn,\n" +" float4* translationOut, Quaternion* orientationOut)\n" +"{\n" +" *orientationOut = qtInvert(orientationIn);\n" +" *translationOut = qtRotate(*orientationOut, -translationIn);\n" +"}\n" +"bool rayConvex(float4 rayFromLocal, float4 rayToLocal, int numFaces, int faceOffset,\n" +" __global const b3GpuFace* faces, float* hitFraction, float4* hitNormal)\n" +"{\n" +" rayFromLocal.w = 0.f;\n" +" rayToLocal.w = 0.f;\n" +" bool result = true;\n" +" float exitFraction = hitFraction[0];\n" +" float enterFraction = -0.3f;\n" +" float4 curHitNormal = (float4)(0,0,0,0);\n" +" for (int i=0;i= 0.f)\n" +" {\n" +" float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);\n" +" if (exitFraction>fraction)\n" +" {\n" +" exitFraction = fraction;\n" +" }\n" +" } \n" +" } else\n" +" {\n" +" if (toPlaneDist<0.f)\n" +" {\n" +" float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);\n" +" if (enterFraction <= fraction)\n" +" {\n" +" enterFraction = fraction;\n" +" curHitNormal = face.m_plane;\n" +" curHitNormal.w = 0.f;\n" +" }\n" +" } else\n" +" {\n" +" result = false;\n" +" }\n" +" }\n" +" if (exitFraction <= enterFraction)\n" +" result = false;\n" +" }\n" +" if (enterFraction < 0.f)\n" +" {\n" +" result = false;\n" +" }\n" +" if (result)\n" +" { \n" +" hitFraction[0] = enterFraction;\n" +" hitNormal[0] = curHitNormal;\n" +" }\n" +" return result;\n" +"}\n" +"bool sphere_intersect(float4 spherePos, float radius, float4 rayFrom, float4 rayTo, float* hitFraction)\n" +"{\n" +" float4 rs = rayFrom - spherePos;\n" +" rs.w = 0.f;\n" +" float4 rayDir = rayTo-rayFrom;\n" +" rayDir.w = 0.f;\n" +" float A = dot(rayDir,rayDir);\n" +" float B = dot(rs, rayDir);\n" +" float C = dot(rs, rs) - (radius * radius);\n" +" float D = B * B - A*C;\n" +" if (D > 0.0f)\n" +" {\n" +" float t = (-B - sqrt(D))/A;\n" +" if ( (t >= 0.0f) && (t < (*hitFraction)) )\n" +" {\n" +" *hitFraction = t;\n" +" return true;\n" +" }\n" +" }\n" +" return false;\n" +"}\n" +"float4 setInterpolate3(float4 from, float4 to, float t)\n" +"{\n" +" float s = 1.0f - t;\n" +" float4 result;\n" +" result = s * from + t * to;\n" +" result.w = 0.f; \n" +" return result; \n" +"}\n" +"__kernel void rayCastKernel( \n" +" int numRays, \n" +" const __global b3RayInfo* rays, \n" +" __global b3RayHit* hitResults, \n" +" const int numBodies, \n" +" __global Body* bodies,\n" +" __global Collidable* collidables,\n" +" __global const b3GpuFace* faces,\n" +" __global const ConvexPolyhedronCL* convexShapes )\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numRays)\n" +" return;\n" +" hitResults[i].m_hitFraction = 1.f;\n" +" float4 rayFrom = rays[i].m_from;\n" +" float4 rayTo = rays[i].m_to;\n" +" float hitFraction = 1.f;\n" +" float4 hitPoint;\n" +" float4 hitNormal;\n" +" int hitBodyIndex= -1;\n" +" int cachedCollidableIndex = -1;\n" +" Collidable cachedCollidable;\n" +" for (int b=0;b=0)\n" +" {\n" +" hitPoint = setInterpolate3(rayFrom, rayTo,hitFraction);\n" +" hitResults[i].m_hitFraction = hitFraction;\n" +" hitResults[i].m_hitPoint = hitPoint;\n" +" hitResults[i].m_hitNormal = normalize(hitNormal);\n" +" hitResults[i].m_hitResult0 = hitBodyIndex;\n" +" }\n" +"}\n" +"__kernel void findRayRigidPairIndexRanges(__global int2* rayRigidPairs, \n" +" __global int* out_firstRayRigidPairIndexPerRay,\n" +" __global int* out_numRayRigidPairsPerRay,\n" +" int numRayRigidPairs)\n" +"{\n" +" int rayRigidPairIndex = get_global_id(0);\n" +" if (rayRigidPairIndex >= numRayRigidPairs) return;\n" +" \n" +" int rayIndex = rayRigidPairs[rayRigidPairIndex].x;\n" +" \n" +" atomic_min(&out_firstRayRigidPairIndexPerRay[rayIndex], rayRigidPairIndex);\n" +" atomic_inc(&out_numRayRigidPairsPerRay[rayIndex]);\n" +"}\n" +"__kernel void rayCastPairsKernel(const __global b3RayInfo* rays, \n" +" __global b3RayHit* hitResults, \n" +" __global int* firstRayRigidPairIndexPerRay,\n" +" __global int* numRayRigidPairsPerRay,\n" +" \n" +" __global Body* bodies,\n" +" __global Collidable* collidables,\n" +" __global const b3GpuFace* faces,\n" +" __global const ConvexPolyhedronCL* convexShapes,\n" +" \n" +" __global int2* rayRigidPairs,\n" +" int numRays)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i >= numRays) return;\n" +" \n" +" float4 rayFrom = rays[i].m_from;\n" +" float4 rayTo = rays[i].m_to;\n" +" \n" +" hitResults[i].m_hitFraction = 1.f;\n" +" \n" +" float hitFraction = 1.f;\n" +" float4 hitPoint;\n" +" float4 hitNormal;\n" +" int hitBodyIndex = -1;\n" +" \n" +" //\n" +" for(int pair = 0; pair < numRayRigidPairsPerRay[i]; ++pair)\n" +" {\n" +" int rayRigidPairIndex = pair + firstRayRigidPairIndexPerRay[i];\n" +" int b = rayRigidPairs[rayRigidPairIndex].y;\n" +" \n" +" if (hitResults[i].m_hitResult2 == b) continue;\n" +" \n" +" Body body = bodies[b];\n" +" Collidable rigidCollidable = collidables[body.m_collidableIdx];\n" +" \n" +" float4 pos = body.m_pos;\n" +" float4 orn = body.m_quat;\n" +" \n" +" if (rigidCollidable.m_shapeType == SHAPE_CONVEX_HULL)\n" +" {\n" +" float4 invPos = (float4)(0,0,0,0);\n" +" float4 invOrn = (float4)(0,0,0,0);\n" +" float4 rayFromLocal = (float4)(0,0,0,0);\n" +" float4 rayToLocal = (float4)(0,0,0,0);\n" +" invOrn = qtInvert(orn);\n" +" invPos = qtRotate(invOrn, -pos);\n" +" rayFromLocal = qtRotate( invOrn, rayFrom ) + invPos;\n" +" rayToLocal = qtRotate( invOrn, rayTo) + invPos;\n" +" rayFromLocal.w = 0.f;\n" +" rayToLocal.w = 0.f;\n" +" int numFaces = convexShapes[rigidCollidable.m_shapeIndex].m_numFaces;\n" +" int faceOffset = convexShapes[rigidCollidable.m_shapeIndex].m_faceOffset;\n" +" \n" +" if (numFaces && rayConvex(rayFromLocal, rayToLocal, numFaces, faceOffset,faces, &hitFraction, &hitNormal))\n" +" {\n" +" hitBodyIndex = b;\n" +" hitPoint = setInterpolate3(rayFrom, rayTo, hitFraction);\n" +" }\n" +" }\n" +" \n" +" if (rigidCollidable.m_shapeType == SHAPE_SPHERE)\n" +" {\n" +" float radius = rigidCollidable.m_radius;\n" +" \n" +" if (sphere_intersect(pos, radius, rayFrom, rayTo, &hitFraction))\n" +" {\n" +" hitBodyIndex = b;\n" +" hitPoint = setInterpolate3(rayFrom, rayTo, hitFraction);\n" +" hitNormal = (float4) (hitPoint - bodies[b].m_pos);\n" +" }\n" +" }\n" +" }\n" +" \n" +" if (hitBodyIndex >= 0)\n" +" {\n" +" hitResults[i].m_hitFraction = hitFraction;\n" +" hitResults[i].m_hitPoint = hitPoint;\n" +" hitResults[i].m_hitNormal = normalize(hitNormal);\n" +" hitResults[i].m_hitResult0 = hitBodyIndex;\n" +" }\n" +" \n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h new file mode 100644 index 000000000000..c7478f54a1b6 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h @@ -0,0 +1,18 @@ + +#ifndef B3_CONSTRAINT4_h +#define B3_CONSTRAINT4_h +#include "Bullet3Common/b3Vector3.h" + +#include "Bullet3Dynamics/shared/b3ContactConstraint4.h" + + +B3_ATTRIBUTE_ALIGNED16(struct) b3GpuConstraint4 : public b3ContactConstraint4 +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); + + inline void setFrictionCoeff(float value) { m_linear[3] = value; } + inline float getFrictionCoeff() const { return m_linear[3]; } +}; + +#endif //B3_CONSTRAINT4_h + diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp new file mode 100644 index 000000000000..af687b54e9b6 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp @@ -0,0 +1,137 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + +#include "b3GpuGenericConstraint.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + +#include +#include "Bullet3Common/b3Transform.h" + +void b3GpuGenericConstraint::getInfo1 (unsigned int* info,const b3RigidBodyData* bodies) +{ + switch (m_constraintType) + { + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + { + *info = 3; + break; + }; + default: + { + b3Assert(0); + } + }; +} + +void getInfo2Point2Point(b3GpuGenericConstraint* constraint, b3GpuConstraintInfo2* info, const b3RigidBodyData* bodies) +{ + b3Transform trA; + trA.setIdentity(); + trA.setOrigin(bodies[constraint->m_rbA].m_pos); + trA.setRotation(bodies[constraint->m_rbA].m_quat); + + b3Transform trB; + trB.setIdentity(); + trB.setOrigin(bodies[constraint->m_rbB].m_pos); + trB.setRotation(bodies[constraint->m_rbB].m_quat); + + // 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; + + b3Vector3 a1 = trA.getBasis()*constraint->getPivotInA(); + //b3Vector3 a1a = b3QuatRotate(trA.getRotation(),constraint->getPivotInA()); + + { + b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis); + b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip); + b3Vector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + if (info->m_J2linearAxis) + { + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip+1] = -1; + info->m_J2linearAxis[2*info->rowskip+2] = -1; + } + + b3Vector3 a2 = trB.getBasis()*constraint->getPivotInB(); + + { + // b3Vector3 a2n = -a2; + b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis); + b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + + + // set right hand side +// b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp; + b3Scalar currERP = info->erp; + + b3Scalar k = info->fps * currERP; + int j; + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = k * (a2[j] + trB.getOrigin()[j] - a1[j] - trA.getOrigin()[j]); + //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + } +#if 0 + if(m_flags & B3_P2P_FLAGS_CFM) + { + for (j=0; j<3; j++) + { + info->cfm[j*info->rowskip] = m_cfm; + } + } +#endif + +#if 0 + b3Scalar 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; + } + } + info->m_damping = m_setting.m_damping; +#endif + +} + +void b3GpuGenericConstraint::getInfo2 (b3GpuConstraintInfo2* info, const b3RigidBodyData* bodies) +{ + switch (m_constraintType) + { + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + { + getInfo2Point2Point(this,info,bodies); + break; + }; + default: + { + b3Assert(0); + } + }; +} diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h new file mode 100644 index 000000000000..14b3ba7fec70 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h @@ -0,0 +1,132 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + +#ifndef B3_GPU_GENERIC_CONSTRAINT_H +#define B3_GPU_GENERIC_CONSTRAINT_H + +#include "Bullet3Common/b3Quaternion.h" +struct b3RigidBodyData; +enum B3_CONSTRAINT_FLAGS +{ + B3_CONSTRAINT_FLAG_ENABLED=1, +}; + +enum b3GpuGenericConstraintType +{ + B3_GPU_POINT2POINT_CONSTRAINT_TYPE=3, + B3_GPU_FIXED_CONSTRAINT_TYPE=4, +// B3_HINGE_CONSTRAINT_TYPE, +// B3_CONETWIST_CONSTRAINT_TYPE, +// B3_D6_CONSTRAINT_TYPE, +// B3_SLIDER_CONSTRAINT_TYPE, +// B3_CONTACT_CONSTRAINT_TYPE, +// B3_D6_SPRING_CONSTRAINT_TYPE, +// B3_GEAR_CONSTRAINT_TYPE, + + B3_GPU_MAX_CONSTRAINT_TYPE +}; + + + +struct b3GpuConstraintInfo2 +{ + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + b3Scalar fps,erp; + + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + b3Scalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis; + + // elements to jump from one row to the next in J's + int rowskip; + + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + b3Scalar *m_constraintError,*cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + b3Scalar *m_lowerLimit,*m_upperLimit; + + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + int *findex; + // number of solver iterations + int m_numIterations; + + //damping of the velocity + b3Scalar m_damping; +}; + + +B3_ATTRIBUTE_ALIGNED16(struct) b3GpuGenericConstraint +{ + int m_constraintType; + int m_rbA; + int m_rbB; + float m_breakingImpulseThreshold; + + b3Vector3 m_pivotInA; + b3Vector3 m_pivotInB; + b3Quaternion m_relTargetAB; + + int m_flags; + int m_uid; + int m_padding[2]; + + int getRigidBodyA() const + { + return m_rbA; + } + int getRigidBodyB() const + { + return m_rbB; + } + + const b3Vector3& getPivotInA() const + { + return m_pivotInA; + } + + const b3Vector3& getPivotInB() const + { + return m_pivotInB; + } + + int isEnabled() const + { + return m_flags & B3_CONSTRAINT_FLAG_ENABLED; + } + + float getBreakingImpulseThreshold() const + { + return m_breakingImpulseThreshold; + } + + ///internal method used by the constraint solver, don't use them directly + void getInfo1 (unsigned int* info,const b3RigidBodyData* bodies); + + ///internal method used by the constraint solver, don't use them directly + void getInfo2 (b3GpuConstraintInfo2* info, const b3RigidBodyData* bodies); + + +}; + +#endif //B3_GPU_GENERIC_CONSTRAINT_H \ No newline at end of file diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp new file mode 100644 index 000000000000..179dfc4f26e8 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp @@ -0,0 +1,1382 @@ + +#include "b3GpuJacobiContactSolver.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3FillCL.h" //b3Int2 +class b3Vector3; +#include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "Bullet3OpenCL/RigidBody/kernels/solverUtils.h" +#include "Bullet3Common/b3Logging.h" +#include "b3GpuConstraint4.h" +#include "Bullet3Common/shared/b3Int2.h" +#include "Bullet3Common/shared/b3Int4.h" +#define SOLVER_UTILS_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl" + + +struct b3GpuJacobiSolverInternalData +{ + //btRadixSort32CL* m_sort32; + //btBoundSearchCL* m_search; + b3PrefixScanCL* m_scan; + + b3OpenCLArray* m_bodyCount; + b3OpenCLArray* m_contactConstraintOffsets; + b3OpenCLArray* m_offsetSplitBodies; + + b3OpenCLArray* m_deltaLinearVelocities; + b3OpenCLArray* m_deltaAngularVelocities; + + b3AlignedObjectArray m_deltaLinearVelocitiesCPU; + b3AlignedObjectArray m_deltaAngularVelocitiesCPU; + + + + b3OpenCLArray* m_contactConstraints; + + b3FillCL* m_filler; + + + cl_kernel m_countBodiesKernel; + cl_kernel m_contactToConstraintSplitKernel; + cl_kernel m_clearVelocitiesKernel; + cl_kernel m_averageVelocitiesKernel; + cl_kernel m_updateBodyVelocitiesKernel; + cl_kernel m_solveContactKernel; + cl_kernel m_solveFrictionKernel; + + + +}; + + +b3GpuJacobiContactSolver::b3GpuJacobiContactSolver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity) + :m_context(ctx), + m_device(device), + m_queue(queue) +{ + m_data = new b3GpuJacobiSolverInternalData; + m_data->m_scan = new b3PrefixScanCL(m_context,m_device,m_queue); + m_data->m_bodyCount = new b3OpenCLArray(m_context,m_queue); + m_data->m_filler = new b3FillCL(m_context,m_device,m_queue); + m_data->m_contactConstraintOffsets = new b3OpenCLArray(m_context,m_queue); + m_data->m_offsetSplitBodies = new b3OpenCLArray(m_context,m_queue); + m_data->m_contactConstraints = new b3OpenCLArray(m_context,m_queue); + m_data->m_deltaLinearVelocities = new b3OpenCLArray(m_context,m_queue); + m_data->m_deltaAngularVelocities = new b3OpenCLArray(m_context,m_queue); + + cl_int pErrNum; + const char* additionalMacros=""; + const char* solverUtilsSource = solverUtilsCL; + { + cl_program solverUtilsProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverUtilsSource, &pErrNum,additionalMacros, SOLVER_UTILS_KERNEL_PATH); + b3Assert(solverUtilsProg); + m_data->m_countBodiesKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "CountBodiesKernel", &pErrNum, solverUtilsProg,additionalMacros ); + b3Assert(m_data->m_countBodiesKernel); + + m_data->m_contactToConstraintSplitKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "ContactToConstraintSplitKernel", &pErrNum, solverUtilsProg,additionalMacros ); + b3Assert(m_data->m_contactToConstraintSplitKernel); + m_data->m_clearVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "ClearVelocitiesKernel", &pErrNum, solverUtilsProg,additionalMacros ); + b3Assert(m_data->m_clearVelocitiesKernel); + + m_data->m_averageVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "AverageVelocitiesKernel", &pErrNum, solverUtilsProg,additionalMacros ); + b3Assert(m_data->m_averageVelocitiesKernel); + + m_data->m_updateBodyVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "UpdateBodyVelocitiesKernel", &pErrNum, solverUtilsProg,additionalMacros ); + b3Assert(m_data->m_updateBodyVelocitiesKernel); + + + m_data->m_solveContactKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "SolveContactJacobiKernel", &pErrNum, solverUtilsProg,additionalMacros ); + b3Assert(m_data->m_solveContactKernel ); + + m_data->m_solveFrictionKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "SolveFrictionJacobiKernel", &pErrNum, solverUtilsProg,additionalMacros ); + b3Assert(m_data->m_solveFrictionKernel); + } + +} + + +b3GpuJacobiContactSolver::~b3GpuJacobiContactSolver() +{ + clReleaseKernel(m_data->m_solveContactKernel); + clReleaseKernel(m_data->m_solveFrictionKernel); + clReleaseKernel(m_data->m_countBodiesKernel); + clReleaseKernel(m_data->m_contactToConstraintSplitKernel); + clReleaseKernel(m_data->m_averageVelocitiesKernel); + clReleaseKernel(m_data->m_updateBodyVelocitiesKernel); + clReleaseKernel(m_data->m_clearVelocitiesKernel ); + + delete m_data->m_deltaLinearVelocities; + delete m_data->m_deltaAngularVelocities; + delete m_data->m_contactConstraints; + delete m_data->m_offsetSplitBodies; + delete m_data->m_contactConstraintOffsets; + delete m_data->m_bodyCount; + delete m_data->m_filler; + delete m_data->m_scan; + delete m_data; +} + + + +b3Vector3 make_float4(float v) +{ + return b3MakeVector3 (v,v,v); +} + +b3Vector4 make_float4(float x,float y, float z, float w) +{ + return b3MakeVector4 (x,y,z,w); +} + + + static + inline + float calcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1, + const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1) + { + return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1); + } + + + static + inline + void setLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1, + b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1) + { + linear = n; + angular0 = b3Cross(r0, n); + angular1 = -b3Cross(r1, n); + } + + +static __inline void solveContact(b3GpuConstraint4& cs, + const b3Vector3& posA, const b3Vector3& linVelARO, const b3Vector3& angVelARO, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, const b3Vector3& linVelBRO, const b3Vector3& angVelBRO, float invMassB, const b3Matrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4], b3Vector3& dLinVelA, b3Vector3& dAngVelA, b3Vector3& dLinVelB, b3Vector3& dAngVelB) +{ + + + for(int ic=0; ic<4; ic++) + { + // dont necessary because this makes change to 0 + if( cs.m_jacCoeffInv[ic] == 0.f ) continue; + + { + b3Vector3 angular0, angular1, linear; + b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA; + b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB; + setLinearAndAngular( (const b3Vector3 &)cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, linear, angular0, angular1 ); + + float rambdaDt = calcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1, + linVelARO+dLinVelA, angVelARO+dAngVelA, linVelBRO+dLinVelB, angVelBRO+dAngVelB ) + cs.m_b[ic]; + rambdaDt *= cs.m_jacCoeffInv[ic]; + + { + float prevSum = cs.m_appliedRambdaDt[ic]; + float updated = prevSum; + updated += rambdaDt; + updated = b3Max( updated, minRambdaDt[ic] ); + updated = b3Min( updated, maxRambdaDt[ic] ); + rambdaDt = updated - prevSum; + cs.m_appliedRambdaDt[ic] = updated; + } + + b3Vector3 linImp0 = invMassA*linear*rambdaDt; + b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; + b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; + b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; +#ifdef _WIN32 + b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp1.getX())); +#endif + + if (invMassA) + { + dLinVelA += linImp0; + dAngVelA += angImp0; + } + if (invMassB) + { + dLinVelB += linImp1; + dAngVelB += angImp1; + } + } + } +} + + + +void solveContact3(b3GpuConstraint4* cs, + b3Vector3* posAPtr, b3Vector3* linVelA, b3Vector3* angVelA, float invMassA, const b3Matrix3x3& invInertiaA, + b3Vector3* posBPtr, b3Vector3* linVelB, b3Vector3* angVelB, float invMassB, const b3Matrix3x3& invInertiaB, + b3Vector3* dLinVelA, b3Vector3* dAngVelA, b3Vector3* dLinVelB, b3Vector3* dAngVelB) +{ + float minRambdaDt = 0; + float maxRambdaDt = FLT_MAX; + + for(int ic=0; ic<4; ic++) + { + if( cs->m_jacCoeffInv[ic] == 0.f ) continue; + + b3Vector3 angular0, angular1, linear; + b3Vector3 r0 = cs->m_worldPos[ic] - *posAPtr; + b3Vector3 r1 = cs->m_worldPos[ic] - *posBPtr; + setLinearAndAngular( cs->m_linear, r0, r1, linear, angular0, angular1 ); + + float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, + *linVelA+*dLinVelA, *angVelA+*dAngVelA, *linVelB+*dLinVelB, *angVelB+*dAngVelB ) + cs->m_b[ic]; + rambdaDt *= cs->m_jacCoeffInv[ic]; + + { + float prevSum = cs->m_appliedRambdaDt[ic]; + float updated = prevSum; + updated += rambdaDt; + updated = b3Max( updated, minRambdaDt ); + updated = b3Min( updated, maxRambdaDt ); + rambdaDt = updated - prevSum; + cs->m_appliedRambdaDt[ic] = updated; + } + + b3Vector3 linImp0 = invMassA*linear*rambdaDt; + b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; + b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; + b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; + + if (invMassA) + { + *dLinVelA += linImp0; + *dAngVelA += angImp0; + } + if (invMassB) + { + *dLinVelB += linImp1; + *dAngVelB += angImp1; + } + } +} + + +static inline void solveFriction(b3GpuConstraint4& cs, + const b3Vector3& posA, const b3Vector3& linVelARO, const b3Vector3& angVelARO, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, const b3Vector3& linVelBRO, const b3Vector3& angVelBRO, float invMassB, const b3Matrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4], b3Vector3& dLinVelA, b3Vector3& dAngVelA, b3Vector3& dLinVelB, b3Vector3& dAngVelB) +{ + + b3Vector3 linVelA = linVelARO+dLinVelA; + b3Vector3 linVelB = linVelBRO+dLinVelB; + b3Vector3 angVelA = angVelARO+dAngVelA; + b3Vector3 angVelB = angVelBRO+dAngVelB; + + if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return; + const b3Vector3& center = (const b3Vector3&)cs.m_center; + + b3Vector3 n = -(const b3Vector3&)cs.m_linear; + + b3Vector3 tangent[2]; +#if 1 + b3PlaneSpace1 (n, tangent[0],tangent[1]); +#else + b3Vector3 r = cs.m_worldPos[0]-center; + tangent[0] = cross3( n, r ); + tangent[1] = cross3( tangent[0], n ); + tangent[0] = normalize3( tangent[0] ); + tangent[1] = normalize3( tangent[1] ); +#endif + + b3Vector3 angular0, angular1, linear; + b3Vector3 r0 = center - posA; + b3Vector3 r1 = center - posB; + for(int i=0; i<2; i++) + { + setLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 ); + float rambdaDt = calcRelVel(linear, -linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB ); + rambdaDt *= cs.m_fJacCoeffInv[i]; + + { + float prevSum = cs.m_fAppliedRambdaDt[i]; + float updated = prevSum; + updated += rambdaDt; + updated = b3Max( updated, minRambdaDt[i] ); + updated = b3Min( updated, maxRambdaDt[i] ); + rambdaDt = updated - prevSum; + cs.m_fAppliedRambdaDt[i] = updated; + } + + b3Vector3 linImp0 = invMassA*linear*rambdaDt; + b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; + b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; + b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; +#ifdef _WIN32 + b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp1.getX())); +#endif + if (invMassA) + { + dLinVelA += linImp0; + dAngVelA += angImp0; + } + if (invMassB) + { + dLinVelB += linImp1; + dAngVelB += angImp1; + } + } + + { // angular damping for point constraint + b3Vector3 ab = ( posB - posA ).normalized(); + b3Vector3 ac = ( center - posA ).normalized(); + if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) + { + float angNA = b3Dot( n, angVelA ); + float angNB = b3Dot( n, angVelB ); + + if (invMassA) + dAngVelA -= (angNA*0.1f)*n; + if (invMassB) + dAngVelB -= (angNB*0.1f)*n; + } + } + +} + + + + +float calcJacCoeff(const b3Vector3& linear0, const b3Vector3& linear1, const b3Vector3& angular0, const b3Vector3& angular1, + float invMass0, const b3Matrix3x3* invInertia0, float invMass1, const b3Matrix3x3* invInertia1, float countA, float countB) +{ + // linear0,1 are normlized + float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0; + + float jmj1 = b3Dot(mtMul3(angular0,*invInertia0), angular0); + float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1; + float jmj3 = b3Dot(mtMul3(angular1,*invInertia1), angular1); + return -1.f/((jmj0+jmj1)*countA+(jmj2+jmj3)*countB); +// return -1.f/((jmj0+jmj1)+(jmj2+jmj3)); + +} + + +void setConstraint4( const b3Vector3& posA, const b3Vector3& linVelA, const b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, const b3Vector3& linVelB, const b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, + b3Contact4* src, float dt, float positionDrift, float positionConstraintCoeff, float countA, float countB, + b3GpuConstraint4* dstC ) +{ + dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit); + dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit); + + float dtInv = 1.f/dt; + for(int ic=0; ic<4; ic++) + { + dstC->m_appliedRambdaDt[ic] = 0.f; + } + dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f; + + + dstC->m_linear = src->m_worldNormalOnB; + dstC->m_linear[3] = 0.7f ;//src->getFrictionCoeff() ); + for(int ic=0; ic<4; ic++) + { + b3Vector3 r0 = src->m_worldPosB[ic] - posA; + b3Vector3 r1 = src->m_worldPosB[ic] - posB; + + if( ic >= src->m_worldNormalOnB[3] )//npoints + { + dstC->m_jacCoeffInv[ic] = 0.f; + continue; + } + + float relVelN; + { + b3Vector3 linear, angular0, angular1; + setLinearAndAngular(src->m_worldNormalOnB, r0, r1, linear, angular0, angular1); + + dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1, + invMassA, &invInertiaA, invMassB, &invInertiaB ,countA,countB); + + relVelN = calcRelVel(linear, -linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB); + + float e = 0.f;//src->getRestituitionCoeff(); + if( relVelN*relVelN < 0.004f ) + { + e = 0.f; + } + + dstC->m_b[ic] = e*relVelN; + //float penetration = src->m_worldPos[ic].w; + dstC->m_b[ic] += (src->m_worldPosB[ic][3] + positionDrift)*positionConstraintCoeff*dtInv; + dstC->m_appliedRambdaDt[ic] = 0.f; + } + } + + if( src->m_worldNormalOnB[3] > 0 )//npoints + { // prepare friction + b3Vector3 center = make_float4(0.f); + for(int i=0; im_worldNormalOnB[3]; i++) + center += src->m_worldPosB[i]; + center /= (float)src->m_worldNormalOnB[3]; + + b3Vector3 tangent[2]; + b3PlaneSpace1(src->m_worldNormalOnB,tangent[0],tangent[1]); + + b3Vector3 r[2]; + r[0] = center - posA; + r[1] = center - posB; + + for(int i=0; i<2; i++) + { + b3Vector3 linear, angular0, angular1; + setLinearAndAngular(tangent[i], r[0], r[1], linear, angular0, angular1); + + dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1, + invMassA, &invInertiaA, invMassB, &invInertiaB ,countA,countB); + dstC->m_fAppliedRambdaDt[i] = 0.f; + } + dstC->m_center = center; + } + + for(int i=0; i<4; i++) + { + if( im_worldNormalOnB[3] ) + { + dstC->m_worldPos[i] = src->m_worldPosB[i]; + } + else + { + dstC->m_worldPos[i] = make_float4(0.f); + } + } +} + + + +void ContactToConstraintKernel(b3Contact4* gContact, b3RigidBodyData* gBodies, b3InertiaData* gShapes, b3GpuConstraint4* gConstraintOut, int nContacts, +float dt, +float positionDrift, +float positionConstraintCoeff, int gIdx, b3AlignedObjectArray& bodyCount +) +{ + //int gIdx = 0;//GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit); + int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit); + + b3Vector3 posA = gBodies[aIdx].m_pos; + b3Vector3 linVelA = gBodies[aIdx].m_linVel; + b3Vector3 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + b3Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertiaWorld;//.m_invInertia; + + b3Vector3 posB = gBodies[bIdx].m_pos; + b3Vector3 linVelB = gBodies[bIdx].m_linVel; + b3Vector3 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + b3Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertiaWorld;//m_invInertia; + + b3GpuConstraint4 cs; + float countA = invMassA ? (float)(bodyCount[aIdx]) : 1; + float countB = invMassB ? (float)(bodyCount[bIdx]) : 1; + setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB, + &gContact[gIdx], dt, positionDrift, positionConstraintCoeff,countA,countB, + &cs ); + + + + cs.m_batchIdx = gContact[gIdx].m_batchIdx; + + gConstraintOut[gIdx] = cs; + } +} + + +void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,const b3JacobiSolverInfo& solverInfo) +{ + B3_PROFILE("b3GpuJacobiContactSolver::solveGroup"); + + b3AlignedObjectArray bodyCount; + bodyCount.resize(numBodies); + for (int i=0;i contactConstraintOffsets; + contactConstraintOffsets.resize(numManifolds); + + + for (int i=0;i offsetSplitBodies; + offsetSplitBodies.resize(numBodies); + unsigned int totalNumSplitBodies; + m_data->m_scan->executeHost(bodyCount,offsetSplitBodies,numBodies,&totalNumSplitBodies); + int numlastBody = bodyCount[numBodies-1]; + totalNumSplitBodies += numlastBody; + printf("totalNumSplitBodies = %d\n",totalNumSplitBodies); + + + + + + b3AlignedObjectArray contactConstraints; + contactConstraints.resize(numManifolds); + + for (int i=0;i deltaLinearVelocities; + b3AlignedObjectArray deltaAngularVelocities; + deltaLinearVelocities.resize(totalNumSplitBodies); + deltaAngularVelocities.resize(totalNumSplitBodies); + for (unsigned int i=0;i* bodies,b3OpenCLArray* inertias,b3OpenCLArray* manifoldPtr,const btJacobiSolverInfo& solverInfo) +{ + b3JacobiSolverInfo solverInfo; + solverInfo.m_fixedBodyIndex = static0Index; + + + B3_PROFILE("b3GpuJacobiContactSolver::solveGroup"); + + //int numBodies = bodies->size(); + int numManifolds = numContacts;//manifoldPtr->size(); + + { + B3_PROFILE("resize"); + m_data->m_bodyCount->resize(numBodies); + } + + unsigned int val=0; + b3Int2 val2; + val2.x=0; + val2.y=0; + + { + B3_PROFILE("m_filler"); + m_data->m_contactConstraintOffsets->resize(numManifolds); + m_data->m_filler->execute(*m_data->m_bodyCount,val,numBodies); + + + m_data->m_filler->execute(*m_data->m_contactConstraintOffsets,val2,numManifolds); + } + + { + B3_PROFILE("m_countBodiesKernel"); + b3LauncherCL launcher(this->m_queue,m_data->m_countBodiesKernel,"m_countBodiesKernel"); + launcher.setBuffer(contactBuf);//manifoldPtr->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL()); + launcher.setConst(numManifolds); + launcher.setConst(solverInfo.m_fixedBodyIndex); + launcher.launch1D(numManifolds); + } + unsigned int totalNumSplitBodies=0; + { + B3_PROFILE("m_scan->execute"); + + m_data->m_offsetSplitBodies->resize(numBodies); + m_data->m_scan->execute(*m_data->m_bodyCount,*m_data->m_offsetSplitBodies,numBodies,&totalNumSplitBodies); + totalNumSplitBodies+=m_data->m_bodyCount->at(numBodies-1); + } + + { + B3_PROFILE("m_data->m_contactConstraints->resize"); + //int numContacts = manifoldPtr->size(); + m_data->m_contactConstraints->resize(numContacts); + } + + { + B3_PROFILE("contactToConstraintSplitKernel"); + b3LauncherCL launcher( m_queue, m_data->m_contactToConstraintSplitKernel,"m_contactToConstraintSplitKernel"); + launcher.setBuffer(contactBuf); + launcher.setBuffer(bodyBuf); + launcher.setBuffer(inertiaBuf); + launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setConst(numContacts); + launcher.setConst(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + launcher.launch1D( numContacts, 64 ); + + } + + + { + B3_PROFILE("m_data->m_deltaLinearVelocities->resize"); + m_data->m_deltaLinearVelocities->resize(totalNumSplitBodies); + m_data->m_deltaAngularVelocities->resize(totalNumSplitBodies); + } + + + + { + B3_PROFILE("m_clearVelocitiesKernel"); + b3LauncherCL launch(m_queue,m_data->m_clearVelocitiesKernel,"m_clearVelocitiesKernel"); + launch.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launch.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launch.setConst(totalNumSplitBodies); + launch.launch1D(totalNumSplitBodies); + clFinish(m_queue); + } + + + int maxIter = solverInfo.m_numIterations; + + for (int iter = 0;iterm_solveContactKernel,"m_solveContactKernel" ); + launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); + launcher.setBuffer(bodyBuf); + launcher.setBuffer(inertiaBuf); + launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL()); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + launcher.setConst(solverInfo.m_fixedBodyIndex); + launcher.setConst(numManifolds); + + launcher.launch1D(numManifolds); + clFinish(m_queue); + } + + + + { + B3_PROFILE("average velocities"); + b3LauncherCL launcher( m_queue, m_data->m_averageVelocitiesKernel,"m_averageVelocitiesKernel"); + launcher.setBuffer(bodyBuf); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(numBodies); + launcher.launch1D(numBodies); + clFinish(m_queue); + } + + + { + B3_PROFILE("m_solveFrictionKernel"); + b3LauncherCL launcher( m_queue, m_data->m_solveFrictionKernel,"m_solveFrictionKernel"); + launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); + launcher.setBuffer(bodyBuf); + launcher.setBuffer(inertiaBuf); + launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL()); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + launcher.setConst(solverInfo.m_fixedBodyIndex); + launcher.setConst(numManifolds); + + launcher.launch1D(numManifolds); + clFinish(m_queue); + } + + + { + B3_PROFILE("average velocities"); + b3LauncherCL launcher( m_queue, m_data->m_averageVelocitiesKernel,"m_averageVelocitiesKernel"); + launcher.setBuffer(bodyBuf); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(numBodies); + launcher.launch1D(numBodies); + clFinish(m_queue); + } + + + + } + + + { + B3_PROFILE("update body velocities"); + b3LauncherCL launcher( m_queue, m_data->m_updateBodyVelocitiesKernel,"m_updateBodyVelocitiesKernel"); + launcher.setBuffer(bodyBuf); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(numBodies); + launcher.launch1D(numBodies); + clFinish(m_queue); + } + + + +} + +#if 0 + +void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray* bodiesGPU,b3OpenCLArray* inertiasGPU,b3OpenCLArray* manifoldPtrGPU,const btJacobiSolverInfo& solverInfo) +{ + + b3AlignedObjectArray bodiesCPU; + bodiesGPU->copyToHost(bodiesCPU); + b3AlignedObjectArray inertiasCPU; + inertiasGPU->copyToHost(inertiasCPU); + b3AlignedObjectArray manifoldPtrCPU; + manifoldPtrGPU->copyToHost(manifoldPtrCPU); + + int numBodiesCPU = bodiesGPU->size(); + int numManifoldsCPU = manifoldPtrGPU->size(); + B3_PROFILE("b3GpuJacobiContactSolver::solveGroupMixed"); + + b3AlignedObjectArray bodyCount; + bodyCount.resize(numBodiesCPU); + for (int i=0;i contactConstraintOffsets; + contactConstraintOffsets.resize(numManifoldsCPU); + + + for (int i=0;i offsetSplitBodies; + offsetSplitBodies.resize(numBodiesCPU); + unsigned int totalNumSplitBodiesCPU; + m_data->m_scan->executeHost(bodyCount,offsetSplitBodies,numBodiesCPU,&totalNumSplitBodiesCPU); + int numlastBody = bodyCount[numBodiesCPU-1]; + totalNumSplitBodiesCPU += numlastBody; + + int numBodies = bodiesGPU->size(); + int numManifolds = manifoldPtrGPU->size(); + + m_data->m_bodyCount->resize(numBodies); + + unsigned int val=0; + b3Int2 val2; + val2.x=0; + val2.y=0; + + { + B3_PROFILE("m_filler"); + m_data->m_contactConstraintOffsets->resize(numManifolds); + m_data->m_filler->execute(*m_data->m_bodyCount,val,numBodies); + + + m_data->m_filler->execute(*m_data->m_contactConstraintOffsets,val2,numManifolds); + } + + { + B3_PROFILE("m_countBodiesKernel"); + b3LauncherCL launcher(this->m_queue,m_data->m_countBodiesKernel); + launcher.setBuffer(manifoldPtrGPU->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL()); + launcher.setConst(numManifolds); + launcher.setConst(solverInfo.m_fixedBodyIndex); + launcher.launch1D(numManifolds); + } + + unsigned int totalNumSplitBodies=0; + m_data->m_offsetSplitBodies->resize(numBodies); + m_data->m_scan->execute(*m_data->m_bodyCount,*m_data->m_offsetSplitBodies,numBodies,&totalNumSplitBodies); + totalNumSplitBodies+=m_data->m_bodyCount->at(numBodies-1); + + if (totalNumSplitBodies != totalNumSplitBodiesCPU) + { + printf("error in totalNumSplitBodies!\n"); + } + + int numContacts = manifoldPtrGPU->size(); + m_data->m_contactConstraints->resize(numContacts); + + + { + B3_PROFILE("contactToConstraintSplitKernel"); + b3LauncherCL launcher( m_queue, m_data->m_contactToConstraintSplitKernel); + launcher.setBuffer(manifoldPtrGPU->getBufferCL()); + launcher.setBuffer(bodiesGPU->getBufferCL()); + launcher.setBuffer(inertiasGPU->getBufferCL()); + launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setConst(numContacts); + launcher.setConst(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + launcher.launch1D( numContacts, 64 ); + clFinish(m_queue); + } + + + + b3AlignedObjectArray contactConstraints; + contactConstraints.resize(numManifoldsCPU); + + for (int i=0;i deltaLinearVelocities; + b3AlignedObjectArray deltaAngularVelocities; + deltaLinearVelocities.resize(totalNumSplitBodiesCPU); + deltaAngularVelocities.resize(totalNumSplitBodiesCPU); + for (int i=0;im_deltaLinearVelocities->resize(totalNumSplitBodies); + m_data->m_deltaAngularVelocities->resize(totalNumSplitBodies); + + + + { + B3_PROFILE("m_clearVelocitiesKernel"); + b3LauncherCL launch(m_queue,m_data->m_clearVelocitiesKernel); + launch.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launch.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launch.setConst(totalNumSplitBodies); + launch.launch1D(totalNumSplitBodies); + } + + + ///!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + + + m_data->m_contactConstraints->copyToHost(contactConstraints); + m_data->m_offsetSplitBodies->copyToHost(offsetSplitBodies); + m_data->m_contactConstraintOffsets->copyToHost(contactConstraintOffsets); + m_data->m_deltaLinearVelocities->copyToHost(deltaLinearVelocities); + m_data->m_deltaAngularVelocities->copyToHost(deltaAngularVelocities); + + for (int iter = 0;iterm_solveContactKernel ); + launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); + launcher.setBuffer(bodiesGPU->getBufferCL()); + launcher.setBuffer(inertiasGPU->getBufferCL()); + launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL()); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + launcher.setConst(solverInfo.m_fixedBodyIndex); + launcher.setConst(numManifolds); + + launcher.launch1D(numManifolds); + clFinish(m_queue); + } + + + int i=0; + for( i=0; im_averageVelocitiesKernel); + launcher.setBuffer(bodiesGPU->getBufferCL()); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(numBodies); + launcher.launch1D(numBodies); + clFinish(m_queue); + } + + //easy + for (int i=0;im_deltaAngularVelocities->copyFromHost(deltaAngularVelocities); + //m_data->m_deltaLinearVelocities->copyFromHost(deltaLinearVelocities); + m_data->m_deltaAngularVelocities->copyToHost(deltaAngularVelocities); + m_data->m_deltaLinearVelocities->copyToHost(deltaLinearVelocities); + +#if 0 + + { + B3_PROFILE("m_solveFrictionKernel"); + b3LauncherCL launcher( m_queue, m_data->m_solveFrictionKernel); + launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); + launcher.setBuffer(bodiesGPU->getBufferCL()); + launcher.setBuffer(inertiasGPU->getBufferCL()); + launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL()); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + launcher.setConst(solverInfo.m_fixedBodyIndex); + launcher.setConst(numManifolds); + + launcher.launch1D(numManifolds); + clFinish(m_queue); + } + + //solve friction + + for(int i=0; im_averageVelocitiesKernel); + launcher.setBuffer(bodiesGPU->getBufferCL()); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(numBodies); + launcher.launch1D(numBodies); + clFinish(m_queue); + } + + //easy + for (int i=0;im_updateBodyVelocitiesKernel); + launcher.setBuffer(bodiesGPU->getBufferCL()); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(numBodies); + launcher.launch1D(numBodies); + clFinish(m_queue); + } + + + //easy + for (int i=0;icopyFromHost(bodiesCPU); + + +} +#endif diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h new file mode 100644 index 000000000000..b418f29ec437 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h @@ -0,0 +1,62 @@ + +#ifndef B3_GPU_JACOBI_CONTACT_SOLVER_H +#define B3_GPU_JACOBI_CONTACT_SOLVER_H +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +//#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" + + +//struct b3InertiaData; +//b3InertiaData + +class b3TypedConstraint; + +struct b3JacobiSolverInfo +{ + int m_fixedBodyIndex; + + float m_deltaTime; + float m_positionDrift; + float m_positionConstraintCoeff; + int m_numIterations; + + b3JacobiSolverInfo() + :m_fixedBodyIndex(0), + m_deltaTime(1./60.f), + m_positionDrift( 0.005f ), + m_positionConstraintCoeff( 0.99f ), + m_numIterations(7) + { + } +}; +class b3GpuJacobiContactSolver +{ +protected: + + struct b3GpuJacobiSolverInternalData* m_data; + + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; + +public: + + b3GpuJacobiContactSolver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity); + virtual ~b3GpuJacobiContactSolver(); + + + void solveContacts(int numBodies, cl_mem bodyBuf, cl_mem inertiaBuf, int numContacts, cl_mem contactBuf, const struct b3Config& config, int static0Index); + void solveGroupHost(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,struct b3Contact4* manifoldPtr, int numManifolds,const b3JacobiSolverInfo& solverInfo); + //void solveGroupHost(btRigidBodyCL* bodies,b3InertiaData* inertias,int numBodies,btContact4* manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btJacobiSolverInfo& solverInfo); + + //b3Scalar solveGroup(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias, int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + + //void solveGroup(btOpenCLArray* bodies,btOpenCLArray* inertias,btOpenCLArray* manifoldPtr,const btJacobiSolverInfo& solverInfo); + //void solveGroupMixed(btOpenCLArray* bodies,btOpenCLArray* inertias,btOpenCLArray* manifoldPtr,const btJacobiSolverInfo& solverInfo); + +}; +#endif //B3_GPU_JACOBI_CONTACT_SOLVER_H + diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp new file mode 100644 index 000000000000..698fa15f9611 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp @@ -0,0 +1,1107 @@ +#include "b3GpuNarrowPhase.h" + + +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h" +#include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h" +#include +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" +#include "Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.h" +#include "Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h" +#include "Bullet3Geometry/b3AabbUtil.h" +#include "Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h" + +#include "b3GpuNarrowPhaseInternalData.h" +#include "Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h" + + + + +b3GpuNarrowPhase::b3GpuNarrowPhase(cl_context ctx, cl_device_id device, cl_command_queue queue, const b3Config& config) +:m_data(0) ,m_planeBodyIndex(-1),m_static0Index(-1), +m_context(ctx), +m_device(device), +m_queue(queue) +{ + + m_data = new b3GpuNarrowPhaseInternalData(); + m_data->m_currentContactBuffer = 0; + + memset(m_data,0,sizeof(b3GpuNarrowPhaseInternalData)); + + + m_data->m_config = config; + + m_data->m_gpuSatCollision = new GpuSatCollision(ctx,device,queue); + + + m_data->m_triangleConvexPairs = new b3OpenCLArray(m_context,m_queue, config.m_maxTriConvexPairCapacity); + + + //m_data->m_convexPairsOutGPU = new b3OpenCLArray(ctx,queue,config.m_maxBroadphasePairs,false); + //m_data->m_planePairs = new b3OpenCLArray(ctx,queue,config.m_maxBroadphasePairs,false); + + m_data->m_pBufContactOutCPU = new b3AlignedObjectArray(); + m_data->m_pBufContactOutCPU->resize(config.m_maxBroadphasePairs); + m_data->m_bodyBufferCPU = new b3AlignedObjectArray(); + m_data->m_bodyBufferCPU->resize(config.m_maxConvexBodies); + + m_data->m_inertiaBufferCPU = new b3AlignedObjectArray(); + m_data->m_inertiaBufferCPU->resize(config.m_maxConvexBodies); + + m_data->m_pBufContactBuffersGPU[0] = new b3OpenCLArray(ctx,queue, config.m_maxContactCapacity,true); + m_data->m_pBufContactBuffersGPU[1] = new b3OpenCLArray(ctx,queue, config.m_maxContactCapacity,true); + + m_data->m_inertiaBufferGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexBodies,false); + m_data->m_collidablesGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexShapes); + m_data->m_collidablesCPU.reserve(config.m_maxConvexShapes); + + m_data->m_localShapeAABBCPU = new b3AlignedObjectArray; + m_data->m_localShapeAABBGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexShapes); + + + //m_data->m_solverDataGPU = adl::Solver::allocate(ctx,queue, config.m_maxBroadphasePairs,false); + m_data->m_bodyBufferGPU = new b3OpenCLArray(ctx,queue, config.m_maxConvexBodies,false); + + m_data->m_convexFacesGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexShapes*config.m_maxFacesPerShape,false); + m_data->m_convexFaces.reserve(config.m_maxConvexShapes*config.m_maxFacesPerShape); + + m_data->m_gpuChildShapes = new b3OpenCLArray(ctx,queue,config.m_maxCompoundChildShapes,false); + + m_data->m_convexPolyhedraGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexShapes,false); + m_data->m_convexPolyhedra.reserve(config.m_maxConvexShapes); + + m_data->m_uniqueEdgesGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexUniqueEdges,true); + m_data->m_uniqueEdges.reserve(config.m_maxConvexUniqueEdges); + + + + m_data->m_convexVerticesGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexVertices,true); + m_data->m_convexVertices.reserve(config.m_maxConvexVertices); + + m_data->m_convexIndicesGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexIndices,true); + m_data->m_convexIndices.reserve(config.m_maxConvexIndices); + + m_data->m_worldVertsB1GPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexBodies*config.m_maxVerticesPerFace); + m_data->m_clippingFacesOutGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexBodies); + m_data->m_worldNormalsAGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexBodies); + m_data->m_worldVertsA1GPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexBodies*config.m_maxVerticesPerFace); + m_data->m_worldVertsB2GPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexBodies*config.m_maxVerticesPerFace); + + + + m_data->m_convexData = new b3AlignedObjectArray(); + + m_data->m_convexData->resize(config.m_maxConvexShapes); + m_data->m_convexPolyhedra.resize(config.m_maxConvexShapes); + + m_data->m_numAcceleratedShapes = 0; + m_data->m_numAcceleratedRigidBodies = 0; + + + m_data->m_subTreesGPU = new b3OpenCLArray(this->m_context,this->m_queue); + m_data->m_treeNodesGPU = new b3OpenCLArray(this->m_context,this->m_queue); + m_data->m_bvhInfoGPU = new b3OpenCLArray(this->m_context,this->m_queue); + + //m_data->m_contactCGPU = new b3OpenCLArray(ctx,queue,config.m_maxBroadphasePairs,false); + //m_data->m_frictionCGPU = new b3OpenCLArray::allocateFrictionConstraint( m_data->m_deviceCL, config.m_maxBroadphasePairs); + + + +} + + +b3GpuNarrowPhase::~b3GpuNarrowPhase() +{ + delete m_data->m_gpuSatCollision; + + delete m_data->m_triangleConvexPairs; + //delete m_data->m_convexPairsOutGPU; + //delete m_data->m_planePairs; + delete m_data->m_pBufContactOutCPU; + delete m_data->m_bodyBufferCPU; + delete m_data->m_inertiaBufferCPU; + delete m_data->m_pBufContactBuffersGPU[0]; + delete m_data->m_pBufContactBuffersGPU[1]; + + + delete m_data->m_inertiaBufferGPU; + delete m_data->m_collidablesGPU; + delete m_data->m_localShapeAABBCPU; + delete m_data->m_localShapeAABBGPU; + delete m_data->m_bodyBufferGPU; + delete m_data->m_convexFacesGPU; + delete m_data->m_gpuChildShapes; + delete m_data->m_convexPolyhedraGPU; + delete m_data->m_uniqueEdgesGPU; + delete m_data->m_convexVerticesGPU; + delete m_data->m_convexIndicesGPU; + delete m_data->m_worldVertsB1GPU; + delete m_data->m_clippingFacesOutGPU; + delete m_data->m_worldNormalsAGPU; + delete m_data->m_worldVertsA1GPU; + delete m_data->m_worldVertsB2GPU; + + delete m_data->m_bvhInfoGPU; + + for (int i=0;im_bvhData.size();i++) + { + delete m_data->m_bvhData[i]; + } + for (int i=0;im_meshInterfaces.size();i++) + { + delete m_data->m_meshInterfaces[i]; + } + m_data->m_meshInterfaces.clear(); + m_data->m_bvhData.clear(); + delete m_data->m_treeNodesGPU; + delete m_data->m_subTreesGPU; + + + delete m_data->m_convexData; + delete m_data; +} + + +int b3GpuNarrowPhase::allocateCollidable() +{ + int curSize = m_data->m_collidablesCPU.size(); + if (curSizem_config.m_maxConvexShapes) + { + m_data->m_collidablesCPU.expand(); + return curSize; + } + else + { + b3Error("allocateCollidable out-of-range %d\n",m_data->m_config.m_maxConvexShapes); + } + return -1; + +} + + + + + +int b3GpuNarrowPhase::registerSphereShape(float radius) +{ + int collidableIndex = allocateCollidable(); + if (collidableIndex<0) + return collidableIndex; + + + b3Collidable& col = getCollidableCpu(collidableIndex); + col.m_shapeType = SHAPE_SPHERE; + col.m_shapeIndex = 0; + col.m_radius = radius; + + if (col.m_shapeIndex>=0) + { + b3SapAabb aabb; + b3Vector3 myAabbMin=b3MakeVector3(-radius,-radius,-radius); + b3Vector3 myAabbMax=b3MakeVector3(radius,radius,radius); + + aabb.m_min[0] = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x; + aabb.m_min[1] = myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y; + aabb.m_min[2] = myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z; + aabb.m_minIndices[3] = 0; + + aabb.m_max[0] = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x; + aabb.m_max[1] = myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y; + aabb.m_max[2] = myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z; + aabb.m_signedMaxIndices[3] = 0; + + m_data->m_localShapeAABBCPU->push_back(aabb); +// m_data->m_localShapeAABBGPU->push_back(aabb); + clFinish(m_queue); + } + + return collidableIndex; +} + + +int b3GpuNarrowPhase::registerFace(const b3Vector3& faceNormal, float faceConstant) +{ + int faceOffset = m_data->m_convexFaces.size(); + b3GpuFace& face = m_data->m_convexFaces.expand(); + face.m_plane = b3MakeVector3(faceNormal.x,faceNormal.y,faceNormal.z,faceConstant); + return faceOffset; +} + +int b3GpuNarrowPhase::registerPlaneShape(const b3Vector3& planeNormal, float planeConstant) +{ + int collidableIndex = allocateCollidable(); + if (collidableIndex<0) + return collidableIndex; + + + b3Collidable& col = getCollidableCpu(collidableIndex); + col.m_shapeType = SHAPE_PLANE; + col.m_shapeIndex = registerFace(planeNormal,planeConstant); + col.m_radius = planeConstant; + + if (col.m_shapeIndex>=0) + { + b3SapAabb aabb; + aabb.m_min[0] = -1e30f; + aabb.m_min[1] = -1e30f; + aabb.m_min[2] = -1e30f; + aabb.m_minIndices[3] = 0; + + aabb.m_max[0] = 1e30f; + aabb.m_max[1] = 1e30f; + aabb.m_max[2] = 1e30f; + aabb.m_signedMaxIndices[3] = 0; + + m_data->m_localShapeAABBCPU->push_back(aabb); +// m_data->m_localShapeAABBGPU->push_back(aabb); + clFinish(m_queue); + } + + return collidableIndex; +} + + +int b3GpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col) +{ + + m_data->m_convexData->resize(m_data->m_numAcceleratedShapes+1); + m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1); + + + b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1); + convex.mC = convexPtr->mC; + convex.mE = convexPtr->mE; + convex.m_extents= convexPtr->m_extents; + convex.m_localCenter = convexPtr->m_localCenter; + convex.m_radius = convexPtr->m_radius; + + convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size(); + int edgeOffset = m_data->m_uniqueEdges.size(); + convex.m_uniqueEdgesOffset = edgeOffset; + + m_data->m_uniqueEdges.resize(edgeOffset+convex.m_numUniqueEdges); + + //convex data here + int i; + for ( i=0;im_uniqueEdges.size();i++) + { + m_data->m_uniqueEdges[edgeOffset+i] = convexPtr->m_uniqueEdges[i]; + } + + int faceOffset = m_data->m_convexFaces.size(); + convex.m_faceOffset = faceOffset; + convex.m_numFaces = convexPtr->m_faces.size(); + + m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces); + + + for (i=0;im_faces.size();i++) + { + m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0], + convexPtr->m_faces[i].m_plane[1], + convexPtr->m_faces[i].m_plane[2], + convexPtr->m_faces[i].m_plane[3]); + + + int indexOffset = m_data->m_convexIndices.size(); + int numIndices = convexPtr->m_faces[i].m_indices.size(); + m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices; + m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset; + m_data->m_convexIndices.resize(indexOffset+numIndices); + for (int p=0;pm_convexIndices[indexOffset+p] = convexPtr->m_faces[i].m_indices[p]; + } + } + + convex.m_numVertices = convexPtr->m_vertices.size(); + int vertexOffset = m_data->m_convexVertices.size(); + convex.m_vertexOffset =vertexOffset; + + m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices); + for (int i=0;im_vertices.size();i++) + { + m_data->m_convexVertices[vertexOffset+i] = convexPtr->m_vertices[i]; + } + + (*m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr; + + + + return m_data->m_numAcceleratedShapes++; +} + + +int b3GpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling) +{ + b3AlignedObjectArray verts; + + unsigned char* vts = (unsigned char*) vertices; + for (int i=0;iinitializePolyhedralFeatures(&verts[0],verts.size(),merge); + } + + int collidableIndex = registerConvexHullShape(utilPtr); + delete utilPtr; + return collidableIndex; +} + +int b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr) +{ + int collidableIndex = allocateCollidable(); + if (collidableIndex<0) + return collidableIndex; + + b3Collidable& col = getCollidableCpu(collidableIndex); + col.m_shapeType = SHAPE_CONVEX_HULL; + col.m_shapeIndex = -1; + + + { + b3Vector3 localCenter=b3MakeVector3(0,0,0); + for (int i=0;im_vertices.size();i++) + localCenter+=utilPtr->m_vertices[i]; + localCenter*= (1.f/utilPtr->m_vertices.size()); + utilPtr->m_localCenter = localCenter; + + col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr,col); + } + + if (col.m_shapeIndex>=0) + { + b3SapAabb aabb; + + b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f); + b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f); + + for (int i=0;im_vertices.size();i++) + { + myAabbMin.setMin(utilPtr->m_vertices[i]); + myAabbMax.setMax(utilPtr->m_vertices[i]); + } + aabb.m_min[0] = myAabbMin[0]; + aabb.m_min[1] = myAabbMin[1]; + aabb.m_min[2] = myAabbMin[2]; + aabb.m_minIndices[3] = 0; + + aabb.m_max[0] = myAabbMax[0]; + aabb.m_max[1] = myAabbMax[1]; + aabb.m_max[2] = myAabbMax[2]; + aabb.m_signedMaxIndices[3] = 0; + + m_data->m_localShapeAABBCPU->push_back(aabb); +// m_data->m_localShapeAABBGPU->push_back(aabb); + } + + return collidableIndex; + +} + +int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray* childShapes) +{ + + int collidableIndex = allocateCollidable(); + if (collidableIndex<0) + return collidableIndex; + + b3Collidable& col = getCollidableCpu(collidableIndex); + col.m_shapeType = SHAPE_COMPOUND_OF_CONVEX_HULLS; + col.m_shapeIndex = m_data->m_cpuChildShapes.size(); + col.m_compoundBvhIndex = m_data->m_bvhInfoCPU.size(); + + { + b3Assert(col.m_shapeIndex+childShapes->size()m_config.m_maxCompoundChildShapes); + for (int i=0;isize();i++) + { + m_data->m_cpuChildShapes.push_back(childShapes->at(i)); + } + } + + + + col.m_numChildShapes = childShapes->size(); + + + b3SapAabb aabbLocalSpace; + b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f); + b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f); + + b3AlignedObjectArray childLocalAabbs; + childLocalAabbs.resize(childShapes->size()); + + //compute local AABB of the compound of all children + for (int i=0;isize();i++) + { + int childColIndex = childShapes->at(i).m_shapeIndex; + //b3Collidable& childCol = getCollidableCpu(childColIndex); + b3SapAabb aabbLoc =m_data->m_localShapeAABBCPU->at(childColIndex); + + b3Vector3 childLocalAabbMin=b3MakeVector3(aabbLoc.m_min[0],aabbLoc.m_min[1],aabbLoc.m_min[2]); + b3Vector3 childLocalAabbMax=b3MakeVector3(aabbLoc.m_max[0],aabbLoc.m_max[1],aabbLoc.m_max[2]); + b3Vector3 aMin,aMax; + b3Scalar margin(0.f); + b3Transform childTr; + childTr.setIdentity(); + + childTr.setOrigin(childShapes->at(i).m_childPosition); + childTr.setRotation(b3Quaternion(childShapes->at(i).m_childOrientation)); + b3TransformAabb(childLocalAabbMin,childLocalAabbMax,margin,childTr,aMin,aMax); + myAabbMin.setMin(aMin); + myAabbMax.setMax(aMax); + childLocalAabbs[i].m_min[0] = aMin[0]; + childLocalAabbs[i].m_min[1] = aMin[1]; + childLocalAabbs[i].m_min[2] = aMin[2]; + childLocalAabbs[i].m_min[3] = 0; + childLocalAabbs[i].m_max[0] = aMax[0]; + childLocalAabbs[i].m_max[1] = aMax[1]; + childLocalAabbs[i].m_max[2] = aMax[2]; + childLocalAabbs[i].m_max[3] = 0; + } + + aabbLocalSpace.m_min[0] = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x; + aabbLocalSpace.m_min[1]= myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y; + aabbLocalSpace.m_min[2]= myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z; + aabbLocalSpace.m_minIndices[3] = 0; + + aabbLocalSpace.m_max[0] = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x; + aabbLocalSpace.m_max[1]= myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y; + aabbLocalSpace.m_max[2]= myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z; + aabbLocalSpace.m_signedMaxIndices[3] = 0; + + m_data->m_localShapeAABBCPU->push_back(aabbLocalSpace); + + + b3QuantizedBvh* bvh = new b3QuantizedBvh; + bvh->setQuantizationValues(myAabbMin,myAabbMax); + QuantizedNodeArray& nodes = bvh->getLeafNodeArray(); + int numNodes = childShapes->size(); + + for (int i=0;iquantize(&node.m_quantizedAabbMin[0],aabbMin,0); + bvh->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); + } + bvh->buildInternal(); + + int numSubTrees = bvh->getSubtreeInfoArray().size(); + + //void setQuantizationValues(const b3Vector3& bvhAabbMin,const b3Vector3& bvhAabbMax,b3Scalar quantizationMargin=b3Scalar(1.0)); + //QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; } + ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized + //void buildInternal(); + + b3BvhInfo bvhInfo; + + bvhInfo.m_aabbMin = bvh->m_bvhAabbMin; + bvhInfo.m_aabbMax = bvh->m_bvhAabbMax; + bvhInfo.m_quantization = bvh->m_bvhQuantization; + bvhInfo.m_numNodes = numNodes; + bvhInfo.m_numSubTrees = numSubTrees; + bvhInfo.m_nodeOffset = m_data->m_treeNodesCPU.size(); + bvhInfo.m_subTreeOffset = m_data->m_subTreesCPU.size(); + + int numNewNodes = bvh->getQuantizedNodeArray().size(); + + for (int i=0;igetQuantizedNodeArray()[i].isLeafNode()) + { + int orgIndex = bvh->getQuantizedNodeArray()[i].getTriangleIndex(); + + b3Vector3 nodeMinVec = bvh->unQuantize(bvh->getQuantizedNodeArray()[i].m_quantizedAabbMin); + b3Vector3 nodeMaxVec = bvh->unQuantize(bvh->getQuantizedNodeArray()[i].m_quantizedAabbMax); + + for (int c=0;c<3;c++) + { + if (childLocalAabbs[orgIndex].m_min[c] < nodeMinVec[c]) + { + printf("min org (%f) and new (%f) ? at i:%d,c:%d\n",childLocalAabbs[i].m_min[c],nodeMinVec[c],i,c); + } + if (childLocalAabbs[orgIndex].m_max[c] > nodeMaxVec[c]) + { + printf("max org (%f) and new (%f) ? at i:%d,c:%d\n",childLocalAabbs[i].m_max[c],nodeMaxVec[c],i,c); + } + + } + } + + } + + m_data->m_bvhInfoCPU.push_back(bvhInfo); + + int numNewSubtrees = bvh->getSubtreeInfoArray().size(); + m_data->m_subTreesCPU.reserve(m_data->m_subTreesCPU.size()+numNewSubtrees); + for (int i=0;im_subTreesCPU.push_back(bvh->getSubtreeInfoArray()[i]); + } + int numNewTreeNodes = bvh->getQuantizedNodeArray().size(); + + for (int i=0;im_treeNodesCPU.push_back(bvh->getQuantizedNodeArray()[i]); + } + +// m_data->m_localShapeAABBGPU->push_back(aabbWS); + clFinish(m_queue); + return collidableIndex; + +} + + +int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray* vertices, b3AlignedObjectArray* indices,const float* scaling1) +{ + + + b3Vector3 scaling=b3MakeVector3(scaling1[0],scaling1[1],scaling1[2]); + + int collidableIndex = allocateCollidable(); + if (collidableIndex<0) + return collidableIndex; + + b3Collidable& col = getCollidableCpu(collidableIndex); + + col.m_shapeType = SHAPE_CONCAVE_TRIMESH; + col.m_shapeIndex = registerConcaveMeshShape(vertices,indices,col,scaling); + col.m_bvhIndex = m_data->m_bvhInfoCPU.size(); + + + b3SapAabb aabb; + b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f); + b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f); + + for (int i=0;isize();i++) + { + b3Vector3 vtx(vertices->at(i)*scaling); + myAabbMin.setMin(vtx); + myAabbMax.setMax(vtx); + } + aabb.m_min[0] = myAabbMin[0]; + aabb.m_min[1] = myAabbMin[1]; + aabb.m_min[2] = myAabbMin[2]; + aabb.m_minIndices[3] = 0; + + aabb.m_max[0] = myAabbMax[0]; + aabb.m_max[1]= myAabbMax[1]; + aabb.m_max[2]= myAabbMax[2]; + aabb.m_signedMaxIndices[3]= 0; + + m_data->m_localShapeAABBCPU->push_back(aabb); +// m_data->m_localShapeAABBGPU->push_back(aabb); + + b3OptimizedBvh* bvh = new b3OptimizedBvh(); + //void b3OptimizedBvh::build(b3StridingMeshInterface* triangles, bool useQuantizedAabbCompression, const b3Vector3& bvhAabbMin, const b3Vector3& bvhAabbMax) + + bool useQuantizedAabbCompression = true; + b3TriangleIndexVertexArray* meshInterface=new b3TriangleIndexVertexArray(); + m_data->m_meshInterfaces.push_back(meshInterface); + b3IndexedMesh mesh; + mesh.m_numTriangles = indices->size()/3; + mesh.m_numVertices = vertices->size(); + mesh.m_vertexBase = (const unsigned char *)&vertices->at(0).x; + mesh.m_vertexStride = sizeof(b3Vector3); + mesh.m_triangleIndexStride = 3 * sizeof(int);// or sizeof(int) + mesh.m_triangleIndexBase = (const unsigned char *)&indices->at(0); + + meshInterface->addIndexedMesh(mesh); + bvh->build(meshInterface, useQuantizedAabbCompression, (b3Vector3&)aabb.m_min, (b3Vector3&)aabb.m_max); + m_data->m_bvhData.push_back(bvh); + int numNodes = bvh->getQuantizedNodeArray().size(); + //b3OpenCLArray* treeNodesGPU = new b3OpenCLArray(this->m_context,this->m_queue,numNodes); + int numSubTrees = bvh->getSubtreeInfoArray().size(); + + b3BvhInfo bvhInfo; + + bvhInfo.m_aabbMin = bvh->m_bvhAabbMin; + bvhInfo.m_aabbMax = bvh->m_bvhAabbMax; + bvhInfo.m_quantization = bvh->m_bvhQuantization; + bvhInfo.m_numNodes = numNodes; + bvhInfo.m_numSubTrees = numSubTrees; + bvhInfo.m_nodeOffset = m_data->m_treeNodesCPU.size(); + bvhInfo.m_subTreeOffset = m_data->m_subTreesCPU.size(); + + m_data->m_bvhInfoCPU.push_back(bvhInfo); + + + int numNewSubtrees = bvh->getSubtreeInfoArray().size(); + m_data->m_subTreesCPU.reserve(m_data->m_subTreesCPU.size()+numNewSubtrees); + for (int i=0;im_subTreesCPU.push_back(bvh->getSubtreeInfoArray()[i]); + } + int numNewTreeNodes = bvh->getQuantizedNodeArray().size(); + + for (int i=0;im_treeNodesCPU.push_back(bvh->getQuantizedNodeArray()[i]); + } + + + + + return collidableIndex; +} + +int b3GpuNarrowPhase::registerConcaveMeshShape(b3AlignedObjectArray* vertices, b3AlignedObjectArray* indices,b3Collidable& col, const float* scaling1) +{ + + + b3Vector3 scaling=b3MakeVector3(scaling1[0],scaling1[1],scaling1[2]); + + m_data->m_convexData->resize(m_data->m_numAcceleratedShapes+1); + m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1); + + + b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1); + convex.mC = b3MakeVector3(0,0,0); + convex.mE = b3MakeVector3(0,0,0); + convex.m_extents= b3MakeVector3(0,0,0); + convex.m_localCenter = b3MakeVector3(0,0,0); + convex.m_radius = 0.f; + + convex.m_numUniqueEdges = 0; + int edgeOffset = m_data->m_uniqueEdges.size(); + convex.m_uniqueEdgesOffset = edgeOffset; + + int faceOffset = m_data->m_convexFaces.size(); + convex.m_faceOffset = faceOffset; + + convex.m_numFaces = indices->size()/3; + m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces); + m_data->m_convexIndices.reserve(convex.m_numFaces*3); + for (int i=0;iat(indices->at(i*3))*scaling); + b3Vector3 vert1(vertices->at(indices->at(i*3+1))*scaling); + b3Vector3 vert2(vertices->at(indices->at(i*3+2))*scaling); + + b3Vector3 normal = ((vert1-vert0).cross(vert2-vert0)).normalize(); + b3Scalar c = -(normal.dot(vert0)); + + m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector4(normal.x,normal.y,normal.z,c); + int indexOffset = m_data->m_convexIndices.size(); + int numIndices = 3; + m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices; + m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset; + m_data->m_convexIndices.resize(indexOffset+numIndices); + for (int p=0;pat(i*3+p); + m_data->m_convexIndices[indexOffset+p] = vi;//convexPtr->m_faces[i].m_indices[p]; + } + } + + convex.m_numVertices = vertices->size(); + int vertexOffset = m_data->m_convexVertices.size(); + convex.m_vertexOffset =vertexOffset; + m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices); + for (int i=0;isize();i++) + { + m_data->m_convexVertices[vertexOffset+i] = vertices->at(i)*scaling; + } + + (*m_data->m_convexData)[m_data->m_numAcceleratedShapes] = 0; + + + return m_data->m_numAcceleratedShapes++; +} + + + +cl_mem b3GpuNarrowPhase::getBodiesGpu() +{ + return (cl_mem)m_data->m_bodyBufferGPU->getBufferCL(); +} + +const struct b3RigidBodyData* b3GpuNarrowPhase::getBodiesCpu() const +{ + return &m_data->m_bodyBufferCPU->at(0); +}; + + + + +int b3GpuNarrowPhase::getNumBodiesGpu() const +{ + return m_data->m_bodyBufferGPU->size(); +} + +cl_mem b3GpuNarrowPhase::getBodyInertiasGpu() +{ + return (cl_mem)m_data->m_inertiaBufferGPU->getBufferCL(); +} + +int b3GpuNarrowPhase::getNumBodyInertiasGpu() const +{ + return m_data->m_inertiaBufferGPU->size(); +} + + +b3Collidable& b3GpuNarrowPhase::getCollidableCpu(int collidableIndex) +{ + return m_data->m_collidablesCPU[collidableIndex]; +} + +const b3Collidable& b3GpuNarrowPhase::getCollidableCpu(int collidableIndex) const +{ + return m_data->m_collidablesCPU[collidableIndex]; +} + +cl_mem b3GpuNarrowPhase::getCollidablesGpu() +{ + return m_data->m_collidablesGPU->getBufferCL(); +} + +const struct b3Collidable* b3GpuNarrowPhase::getCollidablesCpu() const +{ + if (m_data->m_collidablesCPU.size()) + return &m_data->m_collidablesCPU[0]; + return 0; +} + +const struct b3SapAabb* b3GpuNarrowPhase::getLocalSpaceAabbsCpu() const +{ + if (m_data->m_localShapeAABBCPU->size()) + { + return &m_data->m_localShapeAABBCPU->at(0); + } + return 0; +} + + +cl_mem b3GpuNarrowPhase::getAabbLocalSpaceBufferGpu() +{ + return m_data->m_localShapeAABBGPU->getBufferCL(); +} +int b3GpuNarrowPhase::getNumCollidablesGpu() const +{ + return m_data->m_collidablesGPU->size(); +} + + + + + +int b3GpuNarrowPhase::getNumContactsGpu() const +{ + return m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->size(); +} +cl_mem b3GpuNarrowPhase::getContactsGpu() +{ + return m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->getBufferCL(); +} + +const b3Contact4* b3GpuNarrowPhase::getContactsCPU() const +{ + m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->copyToHost(*m_data->m_pBufContactOutCPU); + return &m_data->m_pBufContactOutCPU->at(0); +} + +void b3GpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects) +{ + + cl_mem aabbsLocalSpace = m_data->m_localShapeAABBGPU->getBufferCL(); + + int nContactOut = 0; + + //swap buffer + m_data->m_currentContactBuffer=1-m_data->m_currentContactBuffer; + + //int curSize = m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->size(); + + int maxTriConvexPairCapacity = m_data->m_config.m_maxTriConvexPairCapacity; + int numTriConvexPairsOut=0; + + b3OpenCLArray broadphasePairsGPU(m_context,m_queue); + broadphasePairsGPU.setFromOpenCLBuffer(broadphasePairs,numBroadphasePairs); + + + + + b3OpenCLArray clAabbArrayWorldSpace(this->m_context,this->m_queue); + clAabbArrayWorldSpace.setFromOpenCLBuffer(aabbsWorldSpace,numObjects); + + b3OpenCLArray clAabbArrayLocalSpace(this->m_context,this->m_queue); + clAabbArrayLocalSpace.setFromOpenCLBuffer(aabbsLocalSpace,numObjects); + + m_data->m_gpuSatCollision->computeConvexConvexContactsGPUSAT( + &broadphasePairsGPU, numBroadphasePairs, + m_data->m_bodyBufferGPU, + m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer], + nContactOut, + m_data->m_pBufContactBuffersGPU[1-m_data->m_currentContactBuffer], + m_data->m_config.m_maxContactCapacity, + m_data->m_config.m_compoundPairCapacity, + *m_data->m_convexPolyhedraGPU, + *m_data->m_convexVerticesGPU, + *m_data->m_uniqueEdgesGPU, + *m_data->m_convexFacesGPU, + *m_data->m_convexIndicesGPU, + *m_data->m_collidablesGPU, + *m_data->m_gpuChildShapes, + clAabbArrayWorldSpace, + clAabbArrayLocalSpace, + *m_data->m_worldVertsB1GPU, + *m_data->m_clippingFacesOutGPU, + *m_data->m_worldNormalsAGPU, + *m_data->m_worldVertsA1GPU, + *m_data->m_worldVertsB2GPU, + m_data->m_bvhData, + m_data->m_treeNodesGPU, + m_data->m_subTreesGPU, + m_data->m_bvhInfoGPU, + numObjects, + maxTriConvexPairCapacity, + *m_data->m_triangleConvexPairs, + numTriConvexPairsOut + ); + + /*b3AlignedObjectArray broadphasePairsCPU; + broadphasePairsGPU.copyToHost(broadphasePairsCPU); + printf("checking pairs\n"); + */ +} + +const b3SapAabb& b3GpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const +{ + return m_data->m_localShapeAABBCPU->at(collidableIndex); +} + + + + + +int b3GpuNarrowPhase::registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation , const float* aabbMinPtr, const float* aabbMaxPtr,bool writeToGpu) +{ + b3Vector3 aabbMin=b3MakeVector3(aabbMinPtr[0],aabbMinPtr[1],aabbMinPtr[2]); + b3Vector3 aabbMax=b3MakeVector3(aabbMaxPtr[0],aabbMaxPtr[1],aabbMaxPtr[2]); + + + if (m_data->m_numAcceleratedRigidBodies >= (m_data->m_config.m_maxConvexBodies)) + { + b3Error("registerRigidBody: exceeding the number of rigid bodies, %d > %d \n",m_data->m_numAcceleratedRigidBodies,m_data->m_config.m_maxConvexBodies); + return -1; + } + + m_data->m_bodyBufferCPU->resize(m_data->m_numAcceleratedRigidBodies+1); + + b3RigidBodyData& body = m_data->m_bodyBufferCPU->at(m_data->m_numAcceleratedRigidBodies); + + float friction = 1.f; + float restitution = 0.f; + + body.m_frictionCoeff = friction; + body.m_restituitionCoeff = restitution; + body.m_angVel = b3MakeVector3(0,0,0); + body.m_linVel=b3MakeVector3(0,0,0);//.setZero(); + body.m_pos =b3MakeVector3(position[0],position[1],position[2]); + body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]); + body.m_collidableIdx = collidableIndex; + if (collidableIndex>=0) + { +// body.m_shapeType = m_data->m_collidablesCPU.at(collidableIndex).m_shapeType; + } else + { + // body.m_shapeType = CollisionShape::SHAPE_PLANE; + m_planeBodyIndex = m_data->m_numAcceleratedRigidBodies; + } + //body.m_shapeType = shapeType; + + + body.m_invMass = mass? 1.f/mass : 0.f; + + if (writeToGpu) + { + m_data->m_bodyBufferGPU->copyFromHostPointer(&body,1,m_data->m_numAcceleratedRigidBodies); + } + + b3InertiaData& shapeInfo = m_data->m_inertiaBufferCPU->at(m_data->m_numAcceleratedRigidBodies); + + if (mass==0.f) + { + if (m_data->m_numAcceleratedRigidBodies==0) + m_static0Index = 0; + + shapeInfo.m_initInvInertia.setValue(0,0,0,0,0,0,0,0,0); + shapeInfo.m_invInertiaWorld.setValue(0,0,0,0,0,0,0,0,0); + } else + { + + b3Assert(body.m_collidableIdx>=0); + + //approximate using the aabb of the shape + + //Aabb aabb = (*m_data->m_shapePointers)[shapeIndex]->m_aabb; + b3Vector3 halfExtents = (aabbMax-aabbMin);//*0.5f;//fake larger inertia makes demos more stable ;-) + + b3Vector3 localInertia; + + float lx=2.f*halfExtents[0]; + float ly=2.f*halfExtents[1]; + float lz=2.f*halfExtents[2]; + + localInertia.setValue( (mass/12.0f) * (ly*ly + lz*lz), + (mass/12.0f) * (lx*lx + lz*lz), + (mass/12.0f) * (lx*lx + ly*ly)); + + b3Vector3 invLocalInertia; + invLocalInertia[0] = 1.f/localInertia[0]; + invLocalInertia[1] = 1.f/localInertia[1]; + invLocalInertia[2] = 1.f/localInertia[2]; + invLocalInertia[3] = 0.f; + + shapeInfo.m_initInvInertia.setValue( + invLocalInertia[0], 0, 0, + 0, invLocalInertia[1], 0, + 0, 0, invLocalInertia[2]); + + b3Matrix3x3 m (body.m_quat); + + shapeInfo.m_invInertiaWorld = m.scaled(invLocalInertia) * m.transpose(); + + } + + if (writeToGpu) + m_data->m_inertiaBufferGPU->copyFromHostPointer(&shapeInfo,1,m_data->m_numAcceleratedRigidBodies); + + + + return m_data->m_numAcceleratedRigidBodies++; +} + +int b3GpuNarrowPhase::getNumRigidBodies() const +{ + return m_data->m_numAcceleratedRigidBodies; +} + +void b3GpuNarrowPhase::writeAllBodiesToGpu() +{ + + if (m_data->m_localShapeAABBCPU->size()) + { + m_data->m_localShapeAABBGPU->copyFromHost(*m_data->m_localShapeAABBCPU); + } + + + m_data->m_gpuChildShapes->copyFromHost(m_data->m_cpuChildShapes); + m_data->m_convexFacesGPU->copyFromHost(m_data->m_convexFaces); + m_data->m_convexPolyhedraGPU->copyFromHost(m_data->m_convexPolyhedra); + m_data->m_uniqueEdgesGPU->copyFromHost(m_data->m_uniqueEdges); + m_data->m_convexVerticesGPU->copyFromHost(m_data->m_convexVertices); + m_data->m_convexIndicesGPU->copyFromHost(m_data->m_convexIndices); + m_data->m_bvhInfoGPU->copyFromHost(m_data->m_bvhInfoCPU); + m_data->m_treeNodesGPU->copyFromHost(m_data->m_treeNodesCPU); + m_data->m_subTreesGPU->copyFromHost(m_data->m_subTreesCPU); + + + m_data->m_bodyBufferGPU->resize(m_data->m_numAcceleratedRigidBodies); + m_data->m_inertiaBufferGPU->resize(m_data->m_numAcceleratedRigidBodies); + + if (m_data->m_numAcceleratedRigidBodies) + { + m_data->m_bodyBufferGPU->copyFromHostPointer(&m_data->m_bodyBufferCPU->at(0),m_data->m_numAcceleratedRigidBodies); + m_data->m_inertiaBufferGPU->copyFromHostPointer(&m_data->m_inertiaBufferCPU->at(0),m_data->m_numAcceleratedRigidBodies); + } + if (m_data->m_collidablesCPU.size()) + { + m_data->m_collidablesGPU->copyFromHost(m_data->m_collidablesCPU); + } + + +} + + +void b3GpuNarrowPhase::reset() +{ + m_data->m_numAcceleratedShapes = 0; + m_data->m_numAcceleratedRigidBodies = 0; + this->m_static0Index = -1; + m_data->m_uniqueEdges.resize(0); + m_data->m_convexVertices.resize(0); + m_data->m_convexPolyhedra.resize(0); + m_data->m_convexIndices.resize(0); + m_data->m_cpuChildShapes.resize(0); + m_data->m_convexFaces.resize(0); + m_data->m_collidablesCPU.resize(0); + m_data->m_localShapeAABBCPU->resize(0); + m_data->m_bvhData.resize(0); + m_data->m_treeNodesCPU.resize(0); + m_data->m_subTreesCPU.resize(0); + m_data->m_bvhInfoCPU.resize(0); + +} + + +void b3GpuNarrowPhase::readbackAllBodiesToCpu() +{ + m_data->m_bodyBufferGPU->copyToHostPointer(&m_data->m_bodyBufferCPU->at(0),m_data->m_numAcceleratedRigidBodies); +} + +void b3GpuNarrowPhase::setObjectTransformCpu(float* position, float* orientation , int bodyIndex) +{ + if (bodyIndex>=0 && bodyIndexm_bodyBufferCPU->size()) + { + m_data->m_bodyBufferCPU->at(bodyIndex).m_pos=b3MakeVector3(position[0],position[1],position[2]); + m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]); + } + else + { + b3Warning("setObjectVelocityCpu out of range.\n"); + } +} +void b3GpuNarrowPhase::setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex) +{ + if (bodyIndex>=0 && bodyIndexm_bodyBufferCPU->size()) + { + m_data->m_bodyBufferCPU->at(bodyIndex).m_linVel=b3MakeVector3(linVel[0],linVel[1],linVel[2]); + m_data->m_bodyBufferCPU->at(bodyIndex).m_angVel=b3MakeVector3(angVel[0],angVel[1],angVel[2]); + } else + { + b3Warning("setObjectVelocityCpu out of range.\n"); + } +} + +bool b3GpuNarrowPhase::getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const +{ + if (bodyIndex>=0 && bodyIndexm_bodyBufferCPU->size()) + { + position[0] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.x; + position[1] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.y; + position[2] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.z; + position[3] = 1.f;//or 1 + + orientation[0] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.x; + orientation[1] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.y; + orientation[2] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.z; + orientation[3] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.w; + return true; + } + + b3Warning("getObjectTransformFromCpu out of range.\n"); + return false; +} diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h new file mode 100644 index 000000000000..05ff3fd09e59 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h @@ -0,0 +1,109 @@ +#ifndef B3_GPU_NARROWPHASE_H +#define B3_GPU_NARROWPHASE_H + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Vector3.h" + +class b3GpuNarrowPhase +{ +protected: + + struct b3GpuNarrowPhaseInternalData* m_data; + int m_acceleratedCompanionShapeIndex; + int m_planeBodyIndex; + int m_static0Index; + + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; + + int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr, b3Collidable& col); + int registerConcaveMeshShape(b3AlignedObjectArray* vertices, b3AlignedObjectArray* indices, b3Collidable& col, const float* scaling); + +public: + + + + + b3GpuNarrowPhase(cl_context vtx, cl_device_id dev, cl_command_queue q, const struct b3Config& config); + + virtual ~b3GpuNarrowPhase(void); + + int registerSphereShape(float radius); + int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant); + + int registerCompoundShape(b3AlignedObjectArray* childShapes); + int registerFace(const b3Vector3& faceNormal, float faceConstant); + + int registerConcaveMesh(b3AlignedObjectArray* vertices, b3AlignedObjectArray* indices,const float* scaling); + + //do they need to be merged? + + int registerConvexHullShape(b3ConvexUtility* utilPtr); + int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling); + + int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu); + void setObjectTransform(const float* position, const float* orientation , int bodyIndex); + + void writeAllBodiesToGpu(); + void reset(); + void readbackAllBodiesToCpu(); + bool getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const; + + void setObjectTransformCpu(float* position, float* orientation , int bodyIndex); + void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex); + + + virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects); + + + cl_mem getBodiesGpu(); + const struct b3RigidBodyData* getBodiesCpu() const; + //struct b3RigidBodyData* getBodiesCpu(); + + int getNumBodiesGpu() const; + + cl_mem getBodyInertiasGpu(); + int getNumBodyInertiasGpu() const; + + cl_mem getCollidablesGpu(); + const struct b3Collidable* getCollidablesCpu() const; + int getNumCollidablesGpu() const; + + const struct b3SapAabb* getLocalSpaceAabbsCpu() const; + + const struct b3Contact4* getContactsCPU() const; + + cl_mem getContactsGpu(); + int getNumContactsGpu() const; + + cl_mem getAabbLocalSpaceBufferGpu(); + + int getNumRigidBodies() const; + + int allocateCollidable(); + + int getStatic0Index() const + { + return m_static0Index; + } + b3Collidable& getCollidableCpu(int collidableIndex); + const b3Collidable& getCollidableCpu(int collidableIndex) const; + + const b3GpuNarrowPhaseInternalData* getInternalData() const + { + return m_data; + } + + b3GpuNarrowPhaseInternalData* getInternalData() + { + return m_data; + } + + const struct b3SapAabb& getLocalSpaceAabb(int collidableIndex) const; +}; + +#endif //B3_GPU_NARROWPHASE_H + diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h new file mode 100644 index 000000000000..8a7f1ea85906 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h @@ -0,0 +1,95 @@ + +#ifndef B3_GPU_NARROWPHASE_INTERNAL_DATA_H +#define B3_GPU_NARROWPHASE_INTERNAL_DATA_H + +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" + +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Vector3.h" + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" +#include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h" + +#include "Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h" +#include "Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h" +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Common/shared/b3Int2.h" + + +class b3ConvexUtility; + +struct b3GpuNarrowPhaseInternalData +{ + b3AlignedObjectArray* m_convexData; + + b3AlignedObjectArray m_convexPolyhedra; + b3AlignedObjectArray m_uniqueEdges; + b3AlignedObjectArray m_convexVertices; + b3AlignedObjectArray m_convexIndices; + + b3OpenCLArray* m_convexPolyhedraGPU; + b3OpenCLArray* m_uniqueEdgesGPU; + b3OpenCLArray* m_convexVerticesGPU; + b3OpenCLArray* m_convexIndicesGPU; + + b3OpenCLArray* m_worldVertsB1GPU; + b3OpenCLArray* m_clippingFacesOutGPU; + b3OpenCLArray* m_worldNormalsAGPU; + b3OpenCLArray* m_worldVertsA1GPU; + b3OpenCLArray* m_worldVertsB2GPU; + + b3AlignedObjectArray m_cpuChildShapes; + b3OpenCLArray* m_gpuChildShapes; + + b3AlignedObjectArray m_convexFaces; + b3OpenCLArray* m_convexFacesGPU; + + struct GpuSatCollision* m_gpuSatCollision; + + + b3OpenCLArray* m_triangleConvexPairs; + + + b3OpenCLArray* m_pBufContactBuffersGPU[2]; + int m_currentContactBuffer; + b3AlignedObjectArray* m_pBufContactOutCPU; + + + b3AlignedObjectArray* m_bodyBufferCPU; + b3OpenCLArray* m_bodyBufferGPU; + + b3AlignedObjectArray* m_inertiaBufferCPU; + b3OpenCLArray* m_inertiaBufferGPU; + + int m_numAcceleratedShapes; + int m_numAcceleratedRigidBodies; + + b3AlignedObjectArray m_collidablesCPU; + b3OpenCLArray* m_collidablesGPU; + + b3OpenCLArray* m_localShapeAABBGPU; + b3AlignedObjectArray* m_localShapeAABBCPU; + + b3AlignedObjectArray m_bvhData; + b3AlignedObjectArray m_meshInterfaces; + + b3AlignedObjectArray m_treeNodesCPU; + b3AlignedObjectArray m_subTreesCPU; + + b3AlignedObjectArray m_bvhInfoCPU; + b3OpenCLArray* m_bvhInfoGPU; + + b3OpenCLArray* m_treeNodesGPU; + b3OpenCLArray* m_subTreesGPU; + + + b3Config m_config; + +}; + +#endif //B3_GPU_NARROWPHASE_INTERNAL_DATA_H diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp new file mode 100644 index 000000000000..0d3d50c54813 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp @@ -0,0 +1,1158 @@ + +/* +Copyright (c) 2013 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + + +bool useGpuInitSolverBodies = true; +bool useGpuInfo1 = true; +bool useGpuInfo2= true; +bool useGpuSolveJointConstraintRows=true; +bool useGpuWriteBackVelocities = true; +bool gpuBreakConstraints = true; + +#include "b3GpuPgsConstraintSolver.h" + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + +#include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h" +#include +#include "Bullet3Common/b3AlignedObjectArray.h" +#include //for memset +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" + +#include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h" + +#include "Bullet3OpenCL/RigidBody/kernels/jointSolver.h" //solveConstraintRowsCL +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" + +#define B3_JOINT_SOLVER_PATH "src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl" + + +struct b3GpuPgsJacobiSolverInternalData +{ + + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; + + b3PrefixScanCL* m_prefixScan; + + cl_kernel m_solveJointConstraintRowsKernels; + cl_kernel m_initSolverBodiesKernel; + cl_kernel m_getInfo1Kernel; + cl_kernel m_initBatchConstraintsKernel; + cl_kernel m_getInfo2Kernel; + cl_kernel m_writeBackVelocitiesKernel; + cl_kernel m_breakViolatedConstraintsKernel; + + b3OpenCLArray* m_gpuConstraintRowOffsets; + + b3OpenCLArray* m_gpuSolverBodies; + b3OpenCLArray* m_gpuBatchConstraints; + b3OpenCLArray* m_gpuConstraintRows; + b3OpenCLArray* m_gpuConstraintInfo1; + +// b3AlignedObjectArray m_cpuSolverBodies; + b3AlignedObjectArray m_cpuBatchConstraints; + b3AlignedObjectArray m_cpuConstraintRows; + b3AlignedObjectArray m_cpuConstraintInfo1; + b3AlignedObjectArray m_cpuConstraintRowOffsets; + + b3AlignedObjectArray m_cpuBodies; + b3AlignedObjectArray m_cpuInertias; + + + b3AlignedObjectArray m_cpuConstraints; + + b3AlignedObjectArray m_batchSizes; + + +}; + + +/* +static b3Transform getWorldTransform(b3RigidBodyData* rb) +{ + b3Transform newTrans; + newTrans.setOrigin(rb->m_pos); + newTrans.setRotation(rb->m_quat); + return newTrans; +} + +static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaData* inertia) +{ + return inertia->m_invInertiaWorld; +} + +*/ + +static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb) +{ + return rb->m_linVel; +} + +static const b3Vector3& getAngularVelocity(b3RigidBodyData* rb) +{ + return rb->m_angVel; +} + +b3Vector3 getVelocityInLocalPoint(b3RigidBodyData* rb, const b3Vector3& rel_pos) +{ + //we also calculate lin/ang velocity for kinematic objects + return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos); + +} + + + +b3GpuPgsConstraintSolver::b3GpuPgsConstraintSolver (cl_context ctx, cl_device_id device, cl_command_queue queue,bool usePgs) +{ + m_usePgs = usePgs; + m_gpuData = new b3GpuPgsJacobiSolverInternalData(); + m_gpuData->m_context = ctx; + m_gpuData->m_device = device; + m_gpuData->m_queue = queue; + + m_gpuData->m_prefixScan = new b3PrefixScanCL(ctx,device,queue); + + m_gpuData->m_gpuConstraintRowOffsets = new b3OpenCLArray(m_gpuData->m_context,m_gpuData->m_queue); + + m_gpuData->m_gpuSolverBodies = new b3OpenCLArray(m_gpuData->m_context,m_gpuData->m_queue); + m_gpuData->m_gpuBatchConstraints = new b3OpenCLArray(m_gpuData->m_context,m_gpuData->m_queue); + m_gpuData->m_gpuConstraintRows = new b3OpenCLArray(m_gpuData->m_context,m_gpuData->m_queue); + m_gpuData->m_gpuConstraintInfo1 = new b3OpenCLArray(m_gpuData->m_context,m_gpuData->m_queue); + cl_int errNum=0; + + { + cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,&errNum,"",B3_JOINT_SOLVER_PATH); + //cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_gpuData->m_context,m_gpuData->m_device,0,&errNum,"",B3_JOINT_SOLVER_PATH,true); + b3Assert(errNum==CL_SUCCESS); + m_gpuData->m_solveJointConstraintRowsKernels = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device,solveConstraintRowsCL, "solveJointConstraintRows",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + m_gpuData->m_initSolverBodiesKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"initSolverBodies",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + m_gpuData->m_getInfo1Kernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"getInfo1Kernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + m_gpuData->m_initBatchConstraintsKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"initBatchConstraintsKernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + m_gpuData->m_getInfo2Kernel= b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"getInfo2Kernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + m_gpuData->m_writeBackVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"writeBackVelocitiesKernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + m_gpuData->m_breakViolatedConstraintsKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"breakViolatedConstraintsKernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + + + + + clReleaseProgram(prog); + } + + +} + +b3GpuPgsConstraintSolver::~b3GpuPgsConstraintSolver () +{ + clReleaseKernel(m_gpuData->m_solveJointConstraintRowsKernels); + clReleaseKernel(m_gpuData->m_initSolverBodiesKernel); + clReleaseKernel(m_gpuData->m_getInfo1Kernel); + clReleaseKernel(m_gpuData->m_initBatchConstraintsKernel); + clReleaseKernel(m_gpuData->m_getInfo2Kernel); + clReleaseKernel(m_gpuData->m_writeBackVelocitiesKernel); + clReleaseKernel(m_gpuData->m_breakViolatedConstraintsKernel); + + delete m_gpuData->m_prefixScan; + delete m_gpuData->m_gpuConstraintRowOffsets; + delete m_gpuData->m_gpuSolverBodies; + delete m_gpuData->m_gpuBatchConstraints; + delete m_gpuData->m_gpuConstraintRows; + delete m_gpuData->m_gpuConstraintInfo1; + + delete m_gpuData; +} + +struct b3BatchConstraint +{ + int m_bodyAPtrAndSignBit; + int m_bodyBPtrAndSignBit; + int m_originalConstraintIndex; + int m_batchId; +}; + +static b3AlignedObjectArray batchConstraints; + + +void b3GpuPgsConstraintSolver::recomputeBatches() +{ + m_gpuData->m_batchSizes.clear(); +} + + + + +b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, int numBodies, b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +{ + B3_PROFILE("GPU solveGroupCacheFriendlySetup"); + batchConstraints.resize(numConstraints); + m_gpuData->m_gpuBatchConstraints->resize(numConstraints); + m_staticIdx = -1; + m_maxOverrideNumSolverIterations = 0; + + + /* m_gpuData->m_gpuBodies->resize(numBodies); + m_gpuData->m_gpuBodies->copyFromHostPointer(bodies,numBodies); + + b3OpenCLArray gpuInertias(m_gpuData->m_context,m_gpuData->m_queue); + gpuInertias.resize(numBodies); + gpuInertias.copyFromHostPointer(inertias,numBodies); + */ + + m_gpuData->m_gpuSolverBodies->resize(numBodies); + + + m_tmpSolverBodyPool.resize(numBodies); + { + + if (useGpuInitSolverBodies) + { + B3_PROFILE("m_initSolverBodiesKernel"); + + b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_initSolverBodiesKernel,"m_initSolverBodiesKernel"); + launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); + launcher.setBuffer(gpuBodies->getBufferCL()); + launcher.setConst(numBodies); + launcher.launch1D(numBodies); + clFinish(m_gpuData->m_queue); + + // m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); + } else + { + gpuBodies->copyToHost(m_gpuData->m_cpuBodies); + for (int i=0;im_cpuBodies[i]; + b3GpuSolverBody& solverBody = m_tmpSolverBodyPool[i]; + initSolverBody(i,&solverBody,&body); + solverBody.m_originalBodyIndex = i; + } + m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool); + } + } + +// int totalBodies = 0; + int totalNumRows = 0; + //b3RigidBody* rb0=0,*rb1=0; + //if (1) + { + { + + + // int i; + + m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); + + // b3OpenCLArray gpuConstraints(m_gpuData->m_context,m_gpuData->m_queue); + + + if (useGpuInfo1) + { + B3_PROFILE("info1 and init batchConstraint"); + + m_gpuData->m_gpuConstraintInfo1->resize(numConstraints); + + + if (1) + { + B3_PROFILE("getInfo1Kernel"); + + b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_getInfo1Kernel,"m_getInfo1Kernel"); + launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); + launcher.setBuffer(gpuConstraints->getBufferCL()); + launcher.setConst(numConstraints); + launcher.launch1D(numConstraints); + clFinish(m_gpuData->m_queue); + } + + if (m_gpuData->m_batchSizes.size()==0) + { + B3_PROFILE("initBatchConstraintsKernel"); + + m_gpuData->m_gpuConstraintRowOffsets->resize(numConstraints); + unsigned int total=0; + m_gpuData->m_prefixScan->execute(*m_gpuData->m_gpuConstraintInfo1,*m_gpuData->m_gpuConstraintRowOffsets,numConstraints,&total); + unsigned int lastElem = m_gpuData->m_gpuConstraintInfo1->at(numConstraints-1); + totalNumRows = total+lastElem; + + { + B3_PROFILE("init batch constraints"); + b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_initBatchConstraintsKernel,"m_initBatchConstraintsKernel"); + launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); + launcher.setBuffer(gpuConstraints->getBufferCL()); + launcher.setBuffer(gpuBodies->getBufferCL()); + launcher.setConst(numConstraints); + launcher.launch1D(numConstraints); + clFinish(m_gpuData->m_queue); + } + //assume the batching happens on CPU, so copy the data + m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); + } + } + else + { + totalNumRows = 0; + gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints); + //calculate the total number of contraint rows + for (int i=0;im_cpuConstraints[i].isEnabled()) + { + + m_gpuData->m_cpuConstraints[i].getInfo1(&info1,&m_gpuData->m_cpuBodies[0]); + } else + { + info1 = 0; + } + + totalNumRows += info1; + } + + m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); + m_gpuData->m_gpuConstraintInfo1->copyFromHost(m_tmpConstraintSizesPool); + + } + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); + m_gpuData->m_gpuConstraintRows->resize(totalNumRows); + + // b3GpuConstraintArray verify; + + if (useGpuInfo2) + { + { + B3_PROFILE("getInfo2Kernel"); + b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_getInfo2Kernel,"m_getInfo2Kernel"); + launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL()); + launcher.setBuffer(gpuConstraints->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); + launcher.setBuffer(gpuBodies->getBufferCL()); + launcher.setBuffer(gpuInertias->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); + launcher.setConst(infoGlobal.m_timeStep); + launcher.setConst(infoGlobal.m_erp); + launcher.setConst(infoGlobal.m_globalCfm); + launcher.setConst(infoGlobal.m_damping); + launcher.setConst(infoGlobal.m_numIterations); + launcher.setConst(numConstraints); + launcher.launch1D(numConstraints); + clFinish(m_gpuData->m_queue); + + if (m_gpuData->m_batchSizes.size()==0) + m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); + //m_gpuData->m_gpuConstraintRows->copyToHost(verify); + //m_gpuData->m_gpuConstraintRows->copyToHost(m_tmpSolverNonContactConstraintPool); + + + + } + } + else + { + + gpuInertias->copyToHost(m_gpuData->m_cpuInertias); + + ///setup the b3SolverConstraints + + for (int i=0;im_cpuConstraintRowOffsets[constraintIndex]; + + b3GpuSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[constraintRowOffset]; + b3GpuGenericConstraint& constraint = m_gpuData->m_cpuConstraints[i]; + + b3RigidBodyData& rbA = m_gpuData->m_cpuBodies[ constraint.getRigidBodyA()]; + //b3RigidBody& rbA = constraint.getRigidBodyA(); + // b3RigidBody& rbB = constraint.getRigidBodyB(); + b3RigidBodyData& rbB = m_gpuData->m_cpuBodies[ constraint.getRigidBodyB()]; + + + + int solverBodyIdA = constraint.getRigidBodyA();//getOrInitSolverBody(constraint.getRigidBodyA(),bodies,inertias); + int solverBodyIdB = constraint.getRigidBodyB();//getOrInitSolverBody(constraint.getRigidBodyB(),bodies,inertias); + + b3GpuSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; + b3GpuSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; + + if (rbA.m_invMass) + { + batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA; + } else + { + if (!solverBodyIdA) + m_staticIdx = 0; + batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA; + } + + if (rbB.m_invMass) + { + batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB; + } else + { + if (!solverBodyIdB) + m_staticIdx = 0; + batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB; + } + + + int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; + if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) + m_maxOverrideNumSolverIterations = overrideNumSolverIterations; + + + int j; + for ( j=0;jinternalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + + + b3GpuConstraintInfo2 info2; + info2.fps = 1.f/infoGlobal.m_timeStep; + info2.erp = infoGlobal.m_erp; + info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; + info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; + info2.m_J2linearAxis = 0; + info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; + info2.rowskip = sizeof(b3GpuSolverConstraint)/sizeof(b3Scalar);//check this + ///the size of b3GpuSolverConstraint needs be a multiple of b3Scalar + b3Assert(info2.rowskip*sizeof(b3Scalar)== sizeof(b3GpuSolverConstraint)); + info2.m_constraintError = ¤tConstraintRow->m_rhs; + currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; + info2.m_damping = infoGlobal.m_damping; + info2.cfm = ¤tConstraintRow->m_cfm; + info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; + info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + info2.m_numIterations = infoGlobal.m_numIterations; + m_gpuData->m_cpuConstraints[i].getInfo2(&info2,&m_gpuData->m_cpuBodies[0]); + + ///finalize the constraint setup + for ( j=0;j=m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold()) + { + solverConstraint.m_upperLimit = m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold(); + } + + if (solverConstraint.m_lowerLimit<=-m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold()) + { + solverConstraint.m_lowerLimit = -m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold(); + } + + // solverConstraint.m_originalContactPoint = constraint; + + b3Matrix3x3& invInertiaWorldA= m_gpuData->m_cpuInertias[constraint.getRigidBodyA()].m_invInertiaWorld; + { + + //b3Vector3 angularFactorA(1,1,1); + const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; + solverConstraint.m_angularComponentA = invInertiaWorldA*ftorqueAxis1;//*angularFactorA; + } + + b3Matrix3x3& invInertiaWorldB= m_gpuData->m_cpuInertias[constraint.getRigidBodyB()].m_invInertiaWorld; + { + + const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; + solverConstraint.m_angularComponentB = invInertiaWorldB*ftorqueAxis2;//*constraint.getRigidBodyB().getAngularFactor(); + } + + { + //it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal + //because it gets multiplied iMJlB + b3Vector3 iMJlA = solverConstraint.m_contactNormal*rbA.m_invMass; + b3Vector3 iMJaA = invInertiaWorldA*solverConstraint.m_relpos1CrossNormal; + b3Vector3 iMJlB = solverConstraint.m_contactNormal*rbB.m_invMass;//sign of normal? + b3Vector3 iMJaB = invInertiaWorldB*solverConstraint.m_relpos2CrossNormal; + + b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal); + sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + sum += iMJlB.dot(solverConstraint.m_contactNormal); + sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); + b3Scalar fsum = b3Fabs(sum); + b3Assert(fsum > B3_EPSILON); + solverConstraint.m_jacDiagABInv = fsum>B3_EPSILON?b3Scalar(1.)/sum : 0.f; + } + + + ///fix rhs + ///todo: add force/torque accelerators + { + b3Scalar rel_vel; + b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.m_linVel) + solverConstraint.m_relpos1CrossNormal.dot(rbA.m_angVel); + b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.m_linVel) + solverConstraint.m_relpos2CrossNormal.dot(rbB.m_angVel); + + rel_vel = vel1Dotn+vel2Dotn; + + b3Scalar restitution = 0.f; + b3Scalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 + b3Scalar velocityError = restitution - rel_vel * info2.m_damping; + b3Scalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + b3Scalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_appliedImpulse = 0.f; + + } + } + + } + } + + + + m_gpuData->m_gpuConstraintRows->copyFromHost(m_tmpSolverNonContactConstraintPool); + m_gpuData->m_gpuConstraintInfo1->copyFromHost(m_tmpConstraintSizesPool); + + if (m_gpuData->m_batchSizes.size()==0) + m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); + else + m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); + + m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool); + + + + }//end useGpuInfo2 + + + } + +#ifdef B3_SUPPORT_CONTACT_CONSTRAINTS + { + int i; + + for (i=0;im_deltaLinearVelocity += linearComponent*impulseMagnitude*body->m_linearFactor; + body->m_deltaAngularVelocity += angularComponent*(impulseMagnitude*body->m_angularFactor); +} + + +void resolveSingleConstraintRowGeneric2( b3GpuSolverBody* body1, b3GpuSolverBody* body2, b3GpuSolverConstraint* c) +{ + float deltaImpulse = c->m_rhs-b3Scalar(c->m_appliedImpulse)*c->m_cfm; + float deltaVel1Dotn = b3Dot(c->m_contactNormal,body1->m_deltaLinearVelocity) + b3Dot(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity); + float deltaVel2Dotn = -b3Dot(c->m_contactNormal,body2->m_deltaLinearVelocity) + b3Dot(c->m_relpos2CrossNormal,body2->m_deltaAngularVelocity); + + deltaImpulse -= deltaVel1Dotn*c->m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c->m_jacDiagABInv; + + float sum = b3Scalar(c->m_appliedImpulse) + deltaImpulse; + if (sum < c->m_lowerLimit) + { + deltaImpulse = c->m_lowerLimit-b3Scalar(c->m_appliedImpulse); + c->m_appliedImpulse = c->m_lowerLimit; + } + else if (sum > c->m_upperLimit) + { + deltaImpulse = c->m_upperLimit-b3Scalar(c->m_appliedImpulse); + c->m_appliedImpulse = c->m_upperLimit; + } + else + { + c->m_appliedImpulse = sum; + } + + internalApplyImpulse(body1,c->m_contactNormal*body1->m_invMass,c->m_angularComponentA,deltaImpulse); + internalApplyImpulse(body2,-c->m_contactNormal*body2->m_invMass,c->m_angularComponentB,deltaImpulse); + +} + + + +void b3GpuPgsConstraintSolver::initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyData* rb) +{ + + solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); + solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f); + solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + + b3Assert(rb); +// solverBody->m_worldTransform = getWorldTransform(rb); + solverBody->internalSetInvMass(b3MakeVector3(rb->m_invMass,rb->m_invMass,rb->m_invMass)); + solverBody->m_originalBodyIndex = bodyIndex; + solverBody->m_angularFactor = b3MakeVector3(1,1,1); + solverBody->m_linearFactor = b3MakeVector3(1,1,1); + solverBody->m_linearVelocity = getLinearVelocity(rb); + solverBody->m_angularVelocity = getAngularVelocity(rb); +} + + +void b3GpuPgsConstraintSolver::averageVelocities() +{ +} + + +b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyIterations(b3OpenCLArray* gpuConstraints1,int numConstraints,const b3ContactSolverInfo& infoGlobal) +{ + //only create the batches once. + //@todo: incrementally update batches when constraints are added/activated and/or removed/deactivated + B3_PROFILE("GpuSolveGroupCacheFriendlyIterations"); + + bool createBatches = m_gpuData->m_batchSizes.size()==0; + { + + if (createBatches) + { + + m_gpuData->m_batchSizes.resize(0); + + { + m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); + + B3_PROFILE("batch joints"); + b3Assert(batchConstraints.size()==numConstraints); + int simdWidth =numConstraints+1; + int numBodies = m_tmpSolverBodyPool.size(); + sortConstraintByBatch3( &batchConstraints[0], numConstraints, simdWidth , m_staticIdx, numBodies); + + m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); + + } + } else + { + /*b3AlignedObjectArray cpuCheckBatches; + m_gpuData->m_gpuBatchConstraints->copyToHost(cpuCheckBatches); + b3Assert(cpuCheckBatches.size()==batchConstraints.size()); + printf(".\n"); + */ + //>copyFromHost(batchConstraints); + } + int maxIterations = infoGlobal.m_numIterations; + + bool useBatching = true; + + if (useBatching ) + { + + if (!useGpuSolveJointConstraintRows) + { + B3_PROFILE("copy to host"); + m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); + m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); + m_gpuData->m_gpuConstraintRows->copyToHost(m_tmpSolverNonContactConstraintPool); + m_gpuData->m_gpuConstraintInfo1->copyToHost(m_gpuData->m_cpuConstraintInfo1); + m_gpuData->m_gpuConstraintRowOffsets->copyToHost(m_gpuData->m_cpuConstraintRowOffsets); + gpuConstraints1->copyToHost(m_gpuData->m_cpuConstraints); + + } + + for ( int iteration = 0 ; iteration< maxIterations ; iteration++) + { + + int batchOffset = 0; + int constraintOffset=0; + int numBatches = m_gpuData->m_batchSizes.size(); + for (int bb=0;bbm_batchSizes[bb]; + + + if (useGpuSolveJointConstraintRows) + { + B3_PROFILE("solveJointConstraintRowsKernels"); + + /* + __kernel void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies, + __global b3BatchConstraint* batchConstraints, + __global b3SolverConstraint* rows, + __global unsigned int* numConstraintRowsInfo1, + __global unsigned int* rowOffsets, + __global b3GpuGenericConstraint* constraints, + int batchOffset, + int numConstraintsInBatch*/ + + + b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_solveJointConstraintRowsKernels,"m_solveJointConstraintRowsKernels"); + launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL()); + launcher.setBuffer(gpuConstraints1->getBufferCL());//to detect disabled constraints + launcher.setConst(batchOffset); + launcher.setConst(numConstraintsInBatch); + + launcher.launch1D(numConstraintsInBatch); + + + } else//useGpu + { + + + + for (int b=0;bm_cpuConstraints[c.m_originalConstraintIndex]; + if (constraint->m_flags&B3_CONSTRAINT_FLAG_ENABLED) + { + int numConstraintRows = m_gpuData->m_cpuConstraintInfo1[c.m_originalConstraintIndex]; + int constraintOffset = m_gpuData->m_cpuConstraintRowOffsets[c.m_originalConstraintIndex]; + + for (int jj=0;jjm_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool); + m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); + m_gpuData->m_gpuConstraintRows->copyFromHost(m_tmpSolverNonContactConstraintPool); + } + + //B3_PROFILE("copy to host"); + //m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); + } + //int sz = sizeof(b3GpuSolverBody); + //printf("cpu sizeof(b3GpuSolverBody)=%d\n",sz); + + + + + + } else + { + for ( int iteration = 0 ; iteration< maxIterations ; iteration++) + { + int numJoints = m_tmpSolverNonContactConstraintPool.size(); + for (int j=0;jm_queue); + return 0.f; +} + + + + +static b3AlignedObjectArray bodyUsed; +static b3AlignedObjectArray curUsed; + + + +inline int b3GpuPgsConstraintSolver::sortConstraintByBatch3( b3BatchConstraint* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies) +{ + //int sz = sizeof(b3BatchConstraint); + + B3_PROFILE("sortConstraintByBatch3"); + + static int maxSwaps = 0; + int numSwaps = 0; + + curUsed.resize(2*simdWidth); + + static int maxNumConstraints = 0; + if (maxNumConstraintsm_batchSizes.push_back(nCurrentBatch); + batchIdx ++; + } + } + +#if defined(_DEBUG) + // debugPrintf( "nBatches: %d\n", batchIdx ); + for(int i=0; i* gpuBodies, b3OpenCLArray* gpuInertias, + int numBodies, b3OpenCLArray* gpuConstraints,int numConstraints, const b3ContactSolverInfo& infoGlobal) +{ + + B3_PROFILE("solveJoints"); + //you need to provide at least some bodies + + solveGroupCacheFriendlySetup( gpuBodies, gpuInertias,numBodies,gpuConstraints, numConstraints,infoGlobal); + + solveGroupCacheFriendlyIterations(gpuConstraints, numConstraints,infoGlobal); + + solveGroupCacheFriendlyFinish(gpuBodies, gpuInertias,numBodies, gpuConstraints, numConstraints, infoGlobal); + + return 0.f; +} + +void b3GpuPgsConstraintSolver::solveJoints(int numBodies, b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, + int numConstraints, b3OpenCLArray* gpuConstraints) +{ + b3ContactSolverInfo infoGlobal; + infoGlobal.m_splitImpulse = false; + infoGlobal.m_timeStep = 1.f/60.f; + infoGlobal.m_numIterations = 4;//4; +// infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION; + //infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS; + infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS; + + //if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) + //if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) + + + solveGroup(gpuBodies,gpuInertias,numBodies,gpuConstraints,numConstraints,infoGlobal); + +} + +//b3AlignedObjectArray testBodies; + + +b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias,int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +{ + B3_PROFILE("solveGroupCacheFriendlyFinish"); +// int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); +// int i,j; + + + { + if (gpuBreakConstraints) + { + B3_PROFILE("breakViolatedConstraintsKernel"); + b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_breakViolatedConstraintsKernel,"m_breakViolatedConstraintsKernel"); + launcher.setBuffer(gpuConstraints->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL()); + launcher.setConst(numConstraints); + launcher.launch1D(numConstraints); + } else + { + gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints); + m_gpuData->m_gpuBatchConstraints->copyToHost(m_gpuData->m_cpuBatchConstraints); + m_gpuData->m_gpuConstraintRows->copyToHost(m_gpuData->m_cpuConstraintRows); + gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints); + m_gpuData->m_gpuConstraintInfo1->copyToHost(m_gpuData->m_cpuConstraintInfo1); + m_gpuData->m_gpuConstraintRowOffsets->copyToHost(m_gpuData->m_cpuConstraintRowOffsets); + + for (int cid=0;cidm_cpuConstraintRowOffsets[originalConstraintIndex]; + int numRows = m_gpuData->m_cpuConstraintInfo1[originalConstraintIndex]; + if (numRows) + { + + // printf("cid=%d, breakingThreshold =%f\n",cid,breakingThreshold); + for (int i=0;im_cpuConstraintRows[rowIndex].m_originalConstraintIndex; + float breakingThreshold = m_gpuData->m_cpuConstraints[orgConstraintIndex].m_breakingImpulseThreshold; + // printf("rows[%d].m_appliedImpulse=%f\n",rowIndex,rows[rowIndex].m_appliedImpulse); + if (b3Fabs(m_gpuData->m_cpuConstraintRows[rowIndex].m_appliedImpulse) >= breakingThreshold) + { + + m_gpuData->m_cpuConstraints[orgConstraintIndex].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED; + } + } + } + } + + + gpuConstraints->copyFromHost(m_gpuData->m_cpuConstraints); + } + } + + { + if (useGpuWriteBackVelocities) + { + B3_PROFILE("GPU write back velocities and transforms"); + + b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_writeBackVelocitiesKernel,"m_writeBackVelocitiesKernel"); + launcher.setBuffer(gpuBodies->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); + launcher.setConst(numBodies); + launcher.launch1D(numBodies); + clFinish(m_gpuData->m_queue); +// m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); +// m_gpuData->m_gpuBodies->copyToHostPointer(bodies,numBodies); + //m_gpuData->m_gpuBodies->copyToHost(testBodies); + + } + else + { + B3_PROFILE("CPU write back velocities and transforms"); + + m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); + gpuBodies->copyToHost(m_gpuData->m_cpuBodies); + for ( int i=0;im_cpuBodies[bodyIndex]; + if (body->m_invMass) + { + if (infoGlobal.m_splitImpulse) + m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); + else + m_tmpSolverBodyPool[i].writebackVelocity(); + + if (m_usePgs) + { + body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity; + body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity; + } else + { + b3Assert(0); + } + /* + if (infoGlobal.m_splitImpulse) + { + body->m_pos = m_tmpSolverBodyPool[i].m_worldTransform.getOrigin(); + b3Quaternion orn; + orn = m_tmpSolverBodyPool[i].m_worldTransform.getRotation(); + body->m_quat = orn; + } + */ + } + }//for + + gpuBodies->copyFromHost(m_gpuData->m_cpuBodies); + + } + } + + clFinish(m_gpuData->m_queue); + + m_tmpSolverContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); + + m_tmpSolverBodyPool.resizeNoInitialize(0); + return 0.f; +} diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h new file mode 100644 index 000000000000..ec0e3f73d634 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h @@ -0,0 +1,78 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + +#ifndef B3_GPU_PGS_CONSTRAINT_SOLVER_H +#define B3_GPU_PGS_CONSTRAINT_SOLVER_H + +struct b3Contact4; +struct b3ContactPoint; + + +class b3Dispatcher; + +#include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h" +#include "Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h" +#include "b3GpuSolverBody.h" +#include "b3GpuSolverConstraint.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +struct b3RigidBodyData; +struct b3InertiaData; + +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "b3GpuGenericConstraint.h" + +class b3GpuPgsConstraintSolver +{ +protected: + int m_staticIdx; + struct b3GpuPgsJacobiSolverInternalData* m_gpuData; + protected: + b3AlignedObjectArray m_tmpSolverBodyPool; + b3GpuConstraintArray m_tmpSolverContactConstraintPool; + b3GpuConstraintArray m_tmpSolverNonContactConstraintPool; + b3GpuConstraintArray m_tmpSolverContactFrictionConstraintPool; + b3GpuConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; + + b3AlignedObjectArray m_tmpConstraintSizesPool; + + + bool m_usePgs; + void averageVelocities(); + + int m_maxOverrideNumSolverIterations; + + int m_numSplitImpulseRecoveries; + +// int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias); + void initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyData* rb); + +public: + b3GpuPgsConstraintSolver (cl_context ctx, cl_device_id device, cl_command_queue queue,bool usePgs); + virtual~b3GpuPgsConstraintSolver (); + + virtual b3Scalar solveGroupCacheFriendlyIterations(b3OpenCLArray* gpuConstraints1,int numConstraints,const b3ContactSolverInfo& infoGlobal); + virtual b3Scalar solveGroupCacheFriendlySetup(b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + b3Scalar solveGroupCacheFriendlyFinish(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias,int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + + + b3Scalar solveGroup(b3OpenCLArray* gpuBodies,b3OpenCLArray* gpuInertias, int numBodies,b3OpenCLArray* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + void solveJoints(int numBodies, b3OpenCLArray* gpuBodies, b3OpenCLArray* gpuInertias, + int numConstraints, b3OpenCLArray* gpuConstraints); + + int sortConstraintByBatch3( struct b3BatchConstraint* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies); + void recomputeBatches(); +}; + +#endif //B3_GPU_PGS_CONSTRAINT_SOLVER_H diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp new file mode 100644 index 000000000000..f0b0abd5e006 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp @@ -0,0 +1,1708 @@ + +bool gUseLargeBatches = false; +bool gCpuBatchContacts = false; +bool gCpuSolveConstraint = false; +bool gCpuRadixSort=false; +bool gCpuSetSortData = false; +bool gCpuSortContactsDeterminism = false; +bool gUseCpuCopyConstraints = false; +bool gUseScanHost = false; +bool gReorderContactsOnCpu = false; + +bool optionalSortContactsDeterminism = true; + + +#include "b3GpuPgsContactSolver.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h" + +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3BoundSearchCL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h" +#include +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" +#include "b3Solver.h" + + +#define B3_SOLVER_SETUP_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl" +#define B3_SOLVER_SETUP2_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl" +#define B3_SOLVER_CONTACT_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl" +#define B3_SOLVER_FRICTION_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl" +#define B3_BATCHING_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl" +#define B3_BATCHING_NEW_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl" + +#include "kernels/solverSetup.h" +#include "kernels/solverSetup2.h" +#include "kernels/solveContact.h" +#include "kernels/solveFriction.h" +#include "kernels/batchingKernels.h" +#include "kernels/batchingKernelsNew.h" + + + + + +struct b3GpuBatchingPgsSolverInternalData +{ + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; + int m_pairCapacity; + int m_nIterations; + + b3OpenCLArray* m_contactCGPU; + b3OpenCLArray* m_numConstraints; + b3OpenCLArray* m_offsets; + + b3Solver* m_solverGPU; + + cl_kernel m_batchingKernel; + cl_kernel m_batchingKernelNew; + cl_kernel m_solveContactKernel; + cl_kernel m_solveSingleContactKernel; + cl_kernel m_solveSingleFrictionKernel; + cl_kernel m_solveFrictionKernel; + cl_kernel m_contactToConstraintKernel; + cl_kernel m_setSortDataKernel; + cl_kernel m_reorderContactKernel; + cl_kernel m_copyConstraintKernel; + + cl_kernel m_setDeterminismSortDataBodyAKernel; + cl_kernel m_setDeterminismSortDataBodyBKernel; + cl_kernel m_setDeterminismSortDataChildShapeAKernel; + cl_kernel m_setDeterminismSortDataChildShapeBKernel; + + + + + class b3RadixSort32CL* m_sort32; + class b3BoundSearchCL* m_search; + class b3PrefixScanCL* m_scan; + + b3OpenCLArray* m_sortDataBuffer; + b3OpenCLArray* m_contactBuffer; + + b3OpenCLArray* m_bodyBufferGPU; + b3OpenCLArray* m_inertiaBufferGPU; + b3OpenCLArray* m_pBufContactOutGPU; + + b3OpenCLArray* m_pBufContactOutGPUCopy; + b3OpenCLArray* m_contactKeyValues; + + + b3AlignedObjectArray m_idxBuffer; + b3AlignedObjectArray m_sortData; + b3AlignedObjectArray m_old; + + b3AlignedObjectArray m_batchSizes; + b3OpenCLArray* m_batchSizesGpu; + +}; + + + +b3GpuPgsContactSolver::b3GpuPgsContactSolver(cl_context ctx,cl_device_id device, cl_command_queue q,int pairCapacity) +{ + m_debugOutput=0; + m_data = new b3GpuBatchingPgsSolverInternalData; + m_data->m_context = ctx; + m_data->m_device = device; + m_data->m_queue = q; + m_data->m_pairCapacity = pairCapacity; + m_data->m_nIterations = 4; + m_data->m_batchSizesGpu = new b3OpenCLArray(ctx,q); + m_data->m_bodyBufferGPU = new b3OpenCLArray(ctx,q); + m_data->m_inertiaBufferGPU = new b3OpenCLArray(ctx,q); + m_data->m_pBufContactOutGPU = new b3OpenCLArray(ctx,q); + + m_data->m_pBufContactOutGPUCopy = new b3OpenCLArray(ctx,q); + m_data->m_contactKeyValues = new b3OpenCLArray(ctx,q); + + + m_data->m_solverGPU = new b3Solver(ctx,device,q,512*1024); + + m_data->m_sort32 = new b3RadixSort32CL(ctx,device,m_data->m_queue); + m_data->m_scan = new b3PrefixScanCL(ctx,device,m_data->m_queue,B3_SOLVER_N_CELLS); + m_data->m_search = new b3BoundSearchCL(ctx,device,m_data->m_queue,B3_SOLVER_N_CELLS); + + const int sortSize = B3NEXTMULTIPLEOF( pairCapacity, 512 ); + + m_data->m_sortDataBuffer = new b3OpenCLArray(ctx,m_data->m_queue,sortSize); + m_data->m_contactBuffer = new b3OpenCLArray(ctx,m_data->m_queue); + + m_data->m_numConstraints = new b3OpenCLArray(ctx,m_data->m_queue,B3_SOLVER_N_CELLS); + m_data->m_numConstraints->resize(B3_SOLVER_N_CELLS); + + m_data->m_contactCGPU = new b3OpenCLArray(ctx,q,pairCapacity); + + m_data->m_offsets = new b3OpenCLArray( ctx,m_data->m_queue,B3_SOLVER_N_CELLS); + m_data->m_offsets->resize(B3_SOLVER_N_CELLS); + const char* additionalMacros = ""; + //const char* srcFileNameForCaching=""; + + + + cl_int pErrNum; + const char* batchKernelSource = batchingKernelsCL; + const char* batchKernelNewSource = batchingKernelsNewCL; + const char* solverSetupSource = solverSetupCL; + const char* solverSetup2Source = solverSetup2CL; + const char* solveContactSource = solveContactCL; + const char* solveFrictionSource = solveFrictionCL; + + + { + + cl_program solveContactProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveContactSource, &pErrNum,additionalMacros, B3_SOLVER_CONTACT_KERNEL_PATH); + b3Assert(solveContactProg); + + cl_program solveFrictionProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveFrictionSource, &pErrNum,additionalMacros, B3_SOLVER_FRICTION_KERNEL_PATH); + b3Assert(solveFrictionProg); + + cl_program solverSetup2Prog= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetup2Source, &pErrNum,additionalMacros, B3_SOLVER_SETUP2_KERNEL_PATH); + + + b3Assert(solverSetup2Prog); + + + cl_program solverSetupProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetupSource, &pErrNum,additionalMacros, B3_SOLVER_SETUP_KERNEL_PATH); + b3Assert(solverSetupProg); + + + m_data->m_solveFrictionKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveFrictionSource, "BatchSolveKernelFriction", &pErrNum, solveFrictionProg,additionalMacros ); + b3Assert(m_data->m_solveFrictionKernel); + + m_data->m_solveContactKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveContactSource, "BatchSolveKernelContact", &pErrNum, solveContactProg,additionalMacros ); + b3Assert(m_data->m_solveContactKernel); + + m_data->m_solveSingleContactKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveContactSource, "solveSingleContactKernel", &pErrNum, solveContactProg,additionalMacros ); + b3Assert(m_data->m_solveSingleContactKernel); + + m_data->m_solveSingleFrictionKernel =b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveFrictionSource, "solveSingleFrictionKernel", &pErrNum, solveFrictionProg,additionalMacros ); + b3Assert(m_data->m_solveSingleFrictionKernel); + + m_data->m_contactToConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetupSource, "ContactToConstraintKernel", &pErrNum, solverSetupProg,additionalMacros ); + b3Assert(m_data->m_contactToConstraintKernel); + + m_data->m_setSortDataKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetSortDataKernel", &pErrNum, solverSetup2Prog,additionalMacros ); + b3Assert(m_data->m_setSortDataKernel); + + m_data->m_setDeterminismSortDataBodyAKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetDeterminismSortDataBodyA", &pErrNum, solverSetup2Prog,additionalMacros ); + b3Assert(m_data->m_setDeterminismSortDataBodyAKernel); + + m_data->m_setDeterminismSortDataBodyBKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetDeterminismSortDataBodyB", &pErrNum, solverSetup2Prog,additionalMacros ); + b3Assert(m_data->m_setDeterminismSortDataBodyBKernel); + + m_data->m_setDeterminismSortDataChildShapeAKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetDeterminismSortDataChildShapeA", &pErrNum, solverSetup2Prog,additionalMacros ); + b3Assert(m_data->m_setDeterminismSortDataChildShapeAKernel); + + m_data->m_setDeterminismSortDataChildShapeBKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetDeterminismSortDataChildShapeB", &pErrNum, solverSetup2Prog,additionalMacros ); + b3Assert(m_data->m_setDeterminismSortDataChildShapeBKernel); + + + m_data->m_reorderContactKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "ReorderContactKernel", &pErrNum, solverSetup2Prog,additionalMacros ); + b3Assert(m_data->m_reorderContactKernel); + + + m_data->m_copyConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "CopyConstraintKernel", &pErrNum, solverSetup2Prog,additionalMacros ); + b3Assert(m_data->m_copyConstraintKernel); + + } + + { + cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelSource, &pErrNum,additionalMacros, B3_BATCHING_PATH); + b3Assert(batchingProg); + + m_data->m_batchingKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelSource, "CreateBatches", &pErrNum, batchingProg,additionalMacros ); + b3Assert(m_data->m_batchingKernel); + } + + { + cl_program batchingNewProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelNewSource, &pErrNum,additionalMacros, B3_BATCHING_NEW_PATH); + b3Assert(batchingNewProg); + + m_data->m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesNew", &pErrNum, batchingNewProg,additionalMacros ); + b3Assert(m_data->m_batchingKernelNew); + } + + + + + + + +} + +b3GpuPgsContactSolver::~b3GpuPgsContactSolver() +{ + delete m_data->m_batchSizesGpu; + delete m_data->m_bodyBufferGPU; + delete m_data->m_inertiaBufferGPU; + delete m_data->m_pBufContactOutGPU; + delete m_data->m_pBufContactOutGPUCopy; + delete m_data->m_contactKeyValues; + + + + delete m_data->m_contactCGPU; + delete m_data->m_numConstraints; + delete m_data->m_offsets; + delete m_data->m_sortDataBuffer; + delete m_data->m_contactBuffer; + + delete m_data->m_sort32; + delete m_data->m_scan; + delete m_data->m_search; + delete m_data->m_solverGPU; + + clReleaseKernel(m_data->m_batchingKernel); + clReleaseKernel(m_data->m_batchingKernelNew); + clReleaseKernel(m_data->m_solveSingleContactKernel); + clReleaseKernel(m_data->m_solveSingleFrictionKernel); + clReleaseKernel( m_data->m_solveContactKernel); + clReleaseKernel( m_data->m_solveFrictionKernel); + + clReleaseKernel( m_data->m_contactToConstraintKernel); + clReleaseKernel( m_data->m_setSortDataKernel); + clReleaseKernel( m_data->m_reorderContactKernel); + clReleaseKernel( m_data->m_copyConstraintKernel); + + clReleaseKernel(m_data->m_setDeterminismSortDataBodyAKernel); + clReleaseKernel(m_data->m_setDeterminismSortDataBodyBKernel); + clReleaseKernel(m_data->m_setDeterminismSortDataChildShapeAKernel); + clReleaseKernel(m_data->m_setDeterminismSortDataChildShapeBKernel); + + + + delete m_data; +} + + + +struct b3ConstraintCfg +{ + b3ConstraintCfg( float dt = 0.f ): m_positionDrift( 0.005f ), m_positionConstraintCoeff( 0.2f ), m_dt(dt), m_staticIdx(0) {} + + float m_positionDrift; + float m_positionConstraintCoeff; + float m_dt; + bool m_enableParallelSolve; + float m_batchCellSize; + int m_staticIdx; +}; + + + +void b3GpuPgsContactSolver::solveContactConstraintBatchSizes( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, + b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches,int numIterations, const b3AlignedObjectArray* batchSizes)//const b3OpenCLArray* gpuBatchSizes) +{ + B3_PROFILE("solveContactConstraintBatchSizes"); + int numBatches = batchSizes->size()/B3_MAX_NUM_BATCHES; + for(int iter=0; iterat(cellId*B3_MAX_NUM_BATCHES+ii); + if (!numInBatch) + break; + + { + b3LauncherCL launcher( m_data->m_queue, m_data->m_solveSingleContactKernel,"m_solveSingleContactKernel" ); + launcher.setBuffer(bodyBuf->getBufferCL() ); + launcher.setBuffer(shapeBuf->getBufferCL() ); + launcher.setBuffer( constraint->getBufferCL() ); + launcher.setConst(cellId); + launcher.setConst(offset); + launcher.setConst(numInBatch); + launcher.launch1D(numInBatch); + offset+=numInBatch; + } + } + } + } + + + for(int iter=0; iterat(cellId*B3_MAX_NUM_BATCHES+ii); + if (!numInBatch) + break; + + { + b3LauncherCL launcher( m_data->m_queue, m_data->m_solveSingleFrictionKernel,"m_solveSingleFrictionKernel" ); + launcher.setBuffer(bodyBuf->getBufferCL() ); + launcher.setBuffer(shapeBuf->getBufferCL() ); + launcher.setBuffer( constraint->getBufferCL() ); + launcher.setConst(cellId); + launcher.setConst(offset); + launcher.setConst(numInBatch); + launcher.launch1D(numInBatch); + offset+=numInBatch; + } + } + } + } +} + +void b3GpuPgsContactSolver::solveContactConstraint( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, + b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches,int numIterations, const b3AlignedObjectArray* batchSizes)//,const b3OpenCLArray* gpuBatchSizes) +{ + + //sort the contacts + + + b3Int4 cdata = b3MakeInt4( n, 0, 0, 0 ); + { + + const int nn = B3_SOLVER_N_CELLS; + + cdata.x = 0; + cdata.y = maxNumBatches;//250; + + + int numWorkItems = 64*nn/B3_SOLVER_N_BATCHES; +#ifdef DEBUG_ME + SolverDebugInfo* debugInfo = new SolverDebugInfo[numWorkItems]; + adl::b3OpenCLArray gpuDebugInfo(data->m_device,numWorkItems); +#endif + + + + { + + B3_PROFILE("m_batchSolveKernel iterations"); + for(int iter=0; iterm_queue, m_data->m_solveContactKernel,"m_solveContactKernel" ); +#if 1 + + b3BufferInfoCL bInfo[] = { + + b3BufferInfoCL( bodyBuf->getBufferCL() ), + b3BufferInfoCL( shapeBuf->getBufferCL() ), + b3BufferInfoCL( constraint->getBufferCL() ), + b3BufferInfoCL( m_data->m_solverGPU->m_numConstraints->getBufferCL() ), + b3BufferInfoCL( m_data->m_solverGPU->m_offsets->getBufferCL() ) +#ifdef DEBUG_ME + , b3BufferInfoCL(&gpuDebugInfo) +#endif + }; + + + + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setBuffer( m_data->m_solverGPU->m_batchSizes.getBufferCL()); + //launcher.setConst( cdata.x ); + launcher.setConst( cdata.y ); + launcher.setConst( cdata.z ); + b3Int4 nSplit; + nSplit.x = B3_SOLVER_N_SPLIT_X; + nSplit.y = B3_SOLVER_N_SPLIT_Y; + nSplit.z = B3_SOLVER_N_SPLIT_Z; + + launcher.setConst( nSplit ); + launcher.launch1D( numWorkItems, 64 ); + + +#else + const char* fileName = "m_batchSolveKernel.bin"; + FILE* f = fopen(fileName,"rb"); + if (f) + { + int sizeInBytes=0; + if (fseek(f, 0, SEEK_END) || (sizeInBytes = ftell(f)) == EOF || fseek(f, 0, SEEK_SET)) + { + printf("error, cannot get file size\n"); + exit(0); + } + + unsigned char* buf = (unsigned char*) malloc(sizeInBytes); + fread(buf,sizeInBytes,1,f); + int serializedBytes = launcher.deserializeArgs(buf, sizeInBytes,m_context); + int num = *(int*)&buf[serializedBytes]; + + launcher.launch1D( num); + + //this clFinish is for testing on errors + clFinish(m_queue); + } + +#endif + + +#ifdef DEBUG_ME + clFinish(m_queue); + gpuDebugInfo.read(debugInfo,numWorkItems); + clFinish(m_queue); + for (int i=0;i0) + { + printf("debugInfo[i].m_valInt2 = %d\n",i,debugInfo[i].m_valInt2); + } + + if (debugInfo[i].m_valInt3>0) + { + printf("debugInfo[i].m_valInt3 = %d\n",i,debugInfo[i].m_valInt3); + } + } +#endif //DEBUG_ME + + + } + } + + clFinish(m_data->m_queue); + + + } + + cdata.x = 1; + bool applyFriction=true; + if (applyFriction) + { + B3_PROFILE("m_batchSolveKernel iterations2"); + for(int iter=0; itergetBufferCL() ), + b3BufferInfoCL( shapeBuf->getBufferCL() ), + b3BufferInfoCL( constraint->getBufferCL() ), + b3BufferInfoCL( m_data->m_solverGPU->m_numConstraints->getBufferCL() ), + b3BufferInfoCL( m_data->m_solverGPU->m_offsets->getBufferCL() ) +#ifdef DEBUG_ME + ,b3BufferInfoCL(&gpuDebugInfo) +#endif //DEBUG_ME + }; + b3LauncherCL launcher( m_data->m_queue, m_data->m_solveFrictionKernel,"m_solveFrictionKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setBuffer( m_data->m_solverGPU->m_batchSizes.getBufferCL()); + //launcher.setConst( cdata.x ); + launcher.setConst( cdata.y ); + launcher.setConst( cdata.z ); + + b3Int4 nSplit; + nSplit.x = B3_SOLVER_N_SPLIT_X; + nSplit.y = B3_SOLVER_N_SPLIT_Y; + nSplit.z = B3_SOLVER_N_SPLIT_Z; + + launcher.setConst( nSplit ); + + launcher.launch1D( 64*nn/B3_SOLVER_N_BATCHES, 64 ); + } + } + clFinish(m_data->m_queue); + + } +#ifdef DEBUG_ME + delete[] debugInfo; +#endif //DEBUG_ME + } + + +} + + + + + + + + + + + +static bool sortfnc(const b3SortData& a,const b3SortData& b) +{ + return (a.m_keym_bodyBufferGPU->setFromOpenCLBuffer(bodyBuf,numBodies); + m_data->m_inertiaBufferGPU->setFromOpenCLBuffer(inertiaBuf,numBodies); + m_data->m_pBufContactOutGPU->setFromOpenCLBuffer(contactBuf,numContacts); + + if (optionalSortContactsDeterminism) + { + if (!gCpuSortContactsDeterminism) + { + B3_PROFILE("GPU Sort contact constraints (determinism)"); + + m_data->m_pBufContactOutGPUCopy->resize(numContacts); + m_data->m_contactKeyValues->resize(numContacts); + + m_data->m_pBufContactOutGPU->copyToCL(m_data->m_pBufContactOutGPUCopy->getBufferCL(),numContacts,0,0); + + { + b3LauncherCL launcher(m_data->m_queue, m_data->m_setDeterminismSortDataChildShapeBKernel,"m_setDeterminismSortDataChildShapeBKernel"); + launcher.setBuffer(m_data->m_pBufContactOutGPUCopy->getBufferCL()); + launcher.setBuffer(m_data->m_contactKeyValues->getBufferCL()); + launcher.setConst(numContacts); + launcher.launch1D( numContacts, 64 ); + } + m_data->m_solverGPU->m_sort32->execute(*m_data->m_contactKeyValues); + { + b3LauncherCL launcher(m_data->m_queue, m_data->m_setDeterminismSortDataChildShapeAKernel,"m_setDeterminismSortDataChildShapeAKernel"); + launcher.setBuffer(m_data->m_pBufContactOutGPUCopy->getBufferCL()); + launcher.setBuffer(m_data->m_contactKeyValues->getBufferCL()); + launcher.setConst(numContacts); + launcher.launch1D( numContacts, 64 ); + } + m_data->m_solverGPU->m_sort32->execute(*m_data->m_contactKeyValues); + { + b3LauncherCL launcher(m_data->m_queue, m_data->m_setDeterminismSortDataBodyBKernel,"m_setDeterminismSortDataBodyBKernel"); + launcher.setBuffer(m_data->m_pBufContactOutGPUCopy->getBufferCL()); + launcher.setBuffer(m_data->m_contactKeyValues->getBufferCL()); + launcher.setConst(numContacts); + launcher.launch1D( numContacts, 64 ); + } + + m_data->m_solverGPU->m_sort32->execute(*m_data->m_contactKeyValues); + + { + b3LauncherCL launcher(m_data->m_queue, m_data->m_setDeterminismSortDataBodyAKernel,"m_setDeterminismSortDataBodyAKernel"); + launcher.setBuffer(m_data->m_pBufContactOutGPUCopy->getBufferCL()); + launcher.setBuffer(m_data->m_contactKeyValues->getBufferCL()); + launcher.setConst(numContacts); + launcher.launch1D( numContacts, 64 ); + } + + m_data->m_solverGPU->m_sort32->execute(*m_data->m_contactKeyValues); + + { + B3_PROFILE("gpu reorderContactKernel (determinism)"); + + b3Int4 cdata; + cdata.x = numContacts; + + //b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_data->m_pBufContactOutGPU->getBufferCL() ), b3BufferInfoCL( m_data->m_solverGPU->m_contactBuffer2->getBufferCL()) + // , b3BufferInfoCL( m_data->m_solverGPU->m_sortDataBuffer->getBufferCL()) }; + b3LauncherCL launcher(m_data->m_queue,m_data->m_solverGPU->m_reorderContactKernel,"m_reorderContactKernel"); + launcher.setBuffer(m_data->m_pBufContactOutGPUCopy->getBufferCL()); + launcher.setBuffer(m_data->m_pBufContactOutGPU->getBufferCL()); + launcher.setBuffer(m_data->m_contactKeyValues->getBufferCL()); + launcher.setConst( cdata ); + launcher.launch1D( numContacts, 64 ); + } + + } else + { + B3_PROFILE("CPU Sort contact constraints (determinism)"); + b3AlignedObjectArray cpuConstraints; + m_data->m_pBufContactOutGPU->copyToHost(cpuConstraints); + bool sort = true; + if (sort) + { + cpuConstraints.quickSort(b3ContactCmp); + + for (int i=0;im_pBufContactOutGPU->copyFromHost(cpuConstraints); + if (m_debugOutput==100) + { + for (int i=0;im_pBufContactOutGPU->size(); + + bool useSolver = true; + + + if (useSolver) + { + float dt=1./60.; + b3ConstraintCfg csCfg( dt ); + csCfg.m_enableParallelSolve = true; + csCfg.m_batchCellSize = 6; + csCfg.m_staticIdx = static0Index; + + + b3OpenCLArray* bodyBuf = m_data->m_bodyBufferGPU; + + void* additionalData = 0;//m_data->m_frictionCGPU; + const b3OpenCLArray* shapeBuf = m_data->m_inertiaBufferGPU; + b3OpenCLArray* contactConstraintOut = m_data->m_contactCGPU; + int nContacts = nContactOut; + + + int maxNumBatches = 0; + + if (!gUseLargeBatches) + { + + if( m_data->m_solverGPU->m_contactBuffer2) + { + m_data->m_solverGPU->m_contactBuffer2->resize(nContacts); + } + + if( m_data->m_solverGPU->m_contactBuffer2 == 0 ) + { + m_data->m_solverGPU->m_contactBuffer2 = new b3OpenCLArray(m_data->m_context,m_data->m_queue, nContacts ); + m_data->m_solverGPU->m_contactBuffer2->resize(nContacts); + } + + //clFinish(m_data->m_queue); + + + + { + B3_PROFILE("batching"); + //@todo: just reserve it, without copy of original contact (unless we use warmstarting) + + + + //const b3OpenCLArray* bodyNative = bodyBuf; + + + { + + //b3OpenCLArray* bodyNative = b3OpenCLArrayUtils::map( data->m_device, bodyBuf ); + //b3OpenCLArray* contactNative = b3OpenCLArrayUtils::map( data->m_device, contactsIn ); + + const int sortAlignment = 512; // todo. get this out of sort + if( csCfg.m_enableParallelSolve ) + { + + + int sortSize = B3NEXTMULTIPLEOF( nContacts, sortAlignment ); + + b3OpenCLArray* countsNative = m_data->m_solverGPU->m_numConstraints; + b3OpenCLArray* offsetsNative = m_data->m_solverGPU->m_offsets; + + + if (!gCpuSetSortData) + { // 2. set cell idx + B3_PROFILE("GPU set cell idx"); + struct CB + { + int m_nContacts; + int m_staticIdx; + float m_scale; + b3Int4 m_nSplit; + }; + + b3Assert( sortSize%64 == 0 ); + CB cdata; + cdata.m_nContacts = nContacts; + cdata.m_staticIdx = csCfg.m_staticIdx; + cdata.m_scale = 1.f/csCfg.m_batchCellSize; + cdata.m_nSplit.x = B3_SOLVER_N_SPLIT_X; + cdata.m_nSplit.y = B3_SOLVER_N_SPLIT_Y; + cdata.m_nSplit.z = B3_SOLVER_N_SPLIT_Z; + + m_data->m_solverGPU->m_sortDataBuffer->resize(nContacts); + + + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_data->m_pBufContactOutGPU->getBufferCL() ), b3BufferInfoCL( bodyBuf->getBufferCL()), b3BufferInfoCL( m_data->m_solverGPU->m_sortDataBuffer->getBufferCL()) }; + b3LauncherCL launcher(m_data->m_queue, m_data->m_solverGPU->m_setSortDataKernel,"m_setSortDataKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( cdata.m_nContacts ); + launcher.setConst( cdata.m_scale ); + launcher.setConst(cdata.m_nSplit); + launcher.setConst(cdata.m_staticIdx); + + + launcher.launch1D( sortSize, 64 ); + } else + { + m_data->m_solverGPU->m_sortDataBuffer->resize(nContacts); + b3AlignedObjectArray sortDataCPU; + m_data->m_solverGPU->m_sortDataBuffer->copyToHost(sortDataCPU); + + b3AlignedObjectArray contactCPU; + m_data->m_pBufContactOutGPU->copyToHost(contactCPU); + b3AlignedObjectArray bodiesCPU; + bodyBuf->copyToHost(bodiesCPU); + float scale = 1.f/csCfg.m_batchCellSize; + b3Int4 nSplit; + nSplit.x = B3_SOLVER_N_SPLIT_X; + nSplit.y = B3_SOLVER_N_SPLIT_Y; + nSplit.z = B3_SOLVER_N_SPLIT_Z; + + SetSortDataCPU(&contactCPU[0], &bodiesCPU[0], &sortDataCPU[0], nContacts,scale,nSplit,csCfg.m_staticIdx); + + + m_data->m_solverGPU->m_sortDataBuffer->copyFromHost(sortDataCPU); + } + + + + if (!gCpuRadixSort) + { // 3. sort by cell idx + B3_PROFILE("gpuRadixSort"); + //int n = B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT; + //int sortBit = 32; + //if( n <= 0xffff ) sortBit = 16; + //if( n <= 0xff ) sortBit = 8; + //adl::RadixSort::execute( data->m_sort, *data->m_sortDataBuffer, sortSize ); + //adl::RadixSort32::execute( data->m_sort32, *data->m_sortDataBuffer, sortSize ); + b3OpenCLArray& keyValuesInOut = *(m_data->m_solverGPU->m_sortDataBuffer); + this->m_data->m_solverGPU->m_sort32->execute(keyValuesInOut); + + + + } else + { + b3OpenCLArray& keyValuesInOut = *(m_data->m_solverGPU->m_sortDataBuffer); + b3AlignedObjectArray hostValues; + keyValuesInOut.copyToHost(hostValues); + hostValues.quickSort(sortfnc); + keyValuesInOut.copyFromHost(hostValues); + } + + + if (gUseScanHost) + { + // 4. find entries + B3_PROFILE("cpuBoundSearch"); + b3AlignedObjectArray countsHost; + countsNative->copyToHost(countsHost); + + b3AlignedObjectArray sortDataHost; + m_data->m_solverGPU->m_sortDataBuffer->copyToHost(sortDataHost); + + + //m_data->m_solverGPU->m_search->executeHost(*m_data->m_solverGPU->m_sortDataBuffer,nContacts,*countsNative,B3_SOLVER_N_CELLS,b3BoundSearchCL::COUNT); + m_data->m_solverGPU->m_search->executeHost(sortDataHost,nContacts,countsHost,B3_SOLVER_N_CELLS,b3BoundSearchCL::COUNT); + + countsNative->copyFromHost(countsHost); + + + //adl::BoundSearch::execute( data->m_search, *data->m_sortDataBuffer, nContacts, *countsNative, + // B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT, adl::BoundSearchBase::COUNT ); + + //unsigned int sum; + //m_data->m_solverGPU->m_scan->execute(*countsNative,*offsetsNative, B3_SOLVER_N_CELLS);//,&sum ); + b3AlignedObjectArray offsetsHost; + offsetsHost.resize(offsetsNative->size()); + + + m_data->m_solverGPU->m_scan->executeHost(countsHost,offsetsHost, B3_SOLVER_N_CELLS);//,&sum ); + offsetsNative->copyFromHost(offsetsHost); + + //printf("sum = %d\n",sum); + } else + { + // 4. find entries + B3_PROFILE("gpuBoundSearch"); + m_data->m_solverGPU->m_search->execute(*m_data->m_solverGPU->m_sortDataBuffer,nContacts,*countsNative,B3_SOLVER_N_CELLS,b3BoundSearchCL::COUNT); + m_data->m_solverGPU->m_scan->execute(*countsNative,*offsetsNative, B3_SOLVER_N_CELLS);//,&sum ); + } + + + + + if (nContacts) + { // 5. sort constraints by cellIdx + if (gReorderContactsOnCpu) + { + B3_PROFILE("cpu m_reorderContactKernel"); + b3AlignedObjectArray sortDataHost; + m_data->m_solverGPU->m_sortDataBuffer->copyToHost(sortDataHost); + b3AlignedObjectArray inContacts; + b3AlignedObjectArray outContacts; + m_data->m_pBufContactOutGPU->copyToHost(inContacts); + outContacts.resize(inContacts.size()); + for (int i=0;im_solverGPU->m_contactBuffer2->copyFromHost(outContacts); + + /* "void ReorderContactKernel(__global struct b3Contact4Data* in, __global struct b3Contact4Data* out, __global int2* sortData, int4 cb )\n" + "{\n" + " int nContacts = cb.x;\n" + " int gIdx = GET_GLOBAL_IDX;\n" + " if( gIdx < nContacts )\n" + " {\n" + " int srcIdx = sortData[gIdx].y;\n" + " out[gIdx] = in[srcIdx];\n" + " }\n" + "}\n" + */ + } else + { + B3_PROFILE("gpu m_reorderContactKernel"); + + b3Int4 cdata; + cdata.x = nContacts; + + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( m_data->m_pBufContactOutGPU->getBufferCL() ), + b3BufferInfoCL( m_data->m_solverGPU->m_contactBuffer2->getBufferCL()) + , b3BufferInfoCL( m_data->m_solverGPU->m_sortDataBuffer->getBufferCL()) }; + + b3LauncherCL launcher(m_data->m_queue,m_data->m_solverGPU->m_reorderContactKernel,"m_reorderContactKernel"); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( cdata ); + launcher.launch1D( nContacts, 64 ); + } + } + + + + + } + + } + + //clFinish(m_data->m_queue); + + // { + // b3AlignedObjectArray histogram; + // m_data->m_solverGPU->m_numConstraints->copyToHost(histogram); + // printf(",,,\n"); + // } + + + if (nContacts) + { + + if (gUseCpuCopyConstraints) + { + for (int i=0;im_pBufContactOutGPU->copyFromOpenCLArray(*m_data->m_solverGPU->m_contactBuffer2); + // m_data->m_solverGPU->m_contactBuffer2->getBufferCL(); + // m_data->m_pBufContactOutGPU->getBufferCL() + } + + } else + { + B3_PROFILE("gpu m_copyConstraintKernel"); + b3Int4 cdata; cdata.x = nContacts; + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( m_data->m_solverGPU->m_contactBuffer2->getBufferCL() ), + b3BufferInfoCL( m_data->m_pBufContactOutGPU->getBufferCL() ) + }; + + b3LauncherCL launcher(m_data->m_queue, m_data->m_solverGPU->m_copyConstraintKernel,"m_copyConstraintKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( cdata ); + launcher.launch1D( nContacts, 64 ); + //we use the clFinish for proper benchmark/profile + clFinish(m_data->m_queue); + } + } + + +// bool compareGPU = false; + if (nContacts) + { + if (!gCpuBatchContacts) + { + B3_PROFILE("gpu batchContacts"); + maxNumBatches = 250;//250; + m_data->m_solverGPU->batchContacts( m_data->m_pBufContactOutGPU, nContacts, m_data->m_solverGPU->m_numConstraints, m_data->m_solverGPU->m_offsets, csCfg.m_staticIdx ); + clFinish(m_data->m_queue); + } else + { + B3_PROFILE("cpu batchContacts"); + static b3AlignedObjectArray cpuContacts; + b3OpenCLArray* contactsIn = m_data->m_solverGPU->m_contactBuffer2; + { + B3_PROFILE("copyToHost"); + contactsIn->copyToHost(cpuContacts); + } + b3OpenCLArray* countsNative = m_data->m_solverGPU->m_numConstraints; + b3OpenCLArray* offsetsNative = m_data->m_solverGPU->m_offsets; + + b3AlignedObjectArray nNativeHost; + b3AlignedObjectArray offsetsNativeHost; + + { + B3_PROFILE("countsNative/offsetsNative copyToHost"); + countsNative->copyToHost(nNativeHost); + offsetsNative->copyToHost(offsetsNativeHost); + } + + + int numNonzeroGrid=0; + + if (gUseLargeBatches) + { + m_data->m_batchSizes.resize(B3_MAX_NUM_BATCHES); + int totalNumConstraints = cpuContacts.size(); + //int simdWidth =numBodies+1;//-1;//64;//-1;//32; + int numBatches = sortConstraintByBatch3( &cpuContacts[0], totalNumConstraints, totalNumConstraints+1,csCfg.m_staticIdx ,numBodies,&m_data->m_batchSizes[0]); // on GPU + maxNumBatches = b3Max(numBatches,maxNumBatches); + static int globalMaxBatch = 0; + if (maxNumBatches>globalMaxBatch ) + { + globalMaxBatch = maxNumBatches; + b3Printf("maxNumBatches = %d\n",maxNumBatches); + } + + } else + { + m_data->m_batchSizes.resize(B3_SOLVER_N_CELLS*B3_MAX_NUM_BATCHES); + B3_PROFILE("cpu batch grid"); + for(int i=0; im_batchSizes[i*B3_MAX_NUM_BATCHES]); // on GPU + maxNumBatches = b3Max(numBatches,maxNumBatches); + static int globalMaxBatch = 0; + if (maxNumBatches>globalMaxBatch ) + { + globalMaxBatch = maxNumBatches; + b3Printf("maxNumBatches = %d\n",maxNumBatches); + } + //we use the clFinish for proper benchmark/profile + + } + } + //clFinish(m_data->m_queue); + } + { + B3_PROFILE("m_contactBuffer->copyFromHost"); + m_data->m_solverGPU->m_contactBuffer2->copyFromHost((b3AlignedObjectArray&)cpuContacts); + } + + } + + } + + + + + + } + + + } + + + //printf("maxNumBatches = %d\n", maxNumBatches); + + if (gUseLargeBatches) + { + if (nContacts) + { + B3_PROFILE("cpu batchContacts"); + static b3AlignedObjectArray cpuContacts; +// b3OpenCLArray* contactsIn = m_data->m_solverGPU->m_contactBuffer2; + { + B3_PROFILE("copyToHost"); + m_data->m_pBufContactOutGPU->copyToHost(cpuContacts); + } +// b3OpenCLArray* countsNative = m_data->m_solverGPU->m_numConstraints; +// b3OpenCLArray* offsetsNative = m_data->m_solverGPU->m_offsets; + + + +// int numNonzeroGrid=0; + + { + m_data->m_batchSizes.resize(B3_MAX_NUM_BATCHES); + int totalNumConstraints = cpuContacts.size(); + // int simdWidth =numBodies+1;//-1;//64;//-1;//32; + int numBatches = sortConstraintByBatch3( &cpuContacts[0], totalNumConstraints, totalNumConstraints+1,csCfg.m_staticIdx ,numBodies,&m_data->m_batchSizes[0]); // on GPU + maxNumBatches = b3Max(numBatches,maxNumBatches); + static int globalMaxBatch = 0; + if (maxNumBatches>globalMaxBatch ) + { + globalMaxBatch = maxNumBatches; + b3Printf("maxNumBatches = %d\n",maxNumBatches); + } + + } + { + B3_PROFILE("m_contactBuffer->copyFromHost"); + m_data->m_solverGPU->m_contactBuffer2->copyFromHost((b3AlignedObjectArray&)cpuContacts); + } + + } + + } + + if (nContacts) + { + B3_PROFILE("gpu convertToConstraints"); + m_data->m_solverGPU->convertToConstraints( bodyBuf, + shapeBuf, m_data->m_solverGPU->m_contactBuffer2, + contactConstraintOut, + additionalData, nContacts, + (b3SolverBase::ConstraintCfg&) csCfg ); + clFinish(m_data->m_queue); + } + + + if (1) + { + int numIter = 4; + + m_data->m_solverGPU->m_nIterations = numIter;//10 + if (!gCpuSolveConstraint) + { + B3_PROFILE("GPU solveContactConstraint"); + + /*m_data->m_solverGPU->solveContactConstraint( + m_data->m_bodyBufferGPU, + m_data->m_inertiaBufferGPU, + m_data->m_contactCGPU,0, + nContactOut , + maxNumBatches); + */ + + //m_data->m_batchSizesGpu->copyFromHost(m_data->m_batchSizes); + + if (gUseLargeBatches) + { + solveContactConstraintBatchSizes(m_data->m_bodyBufferGPU, + m_data->m_inertiaBufferGPU, + m_data->m_contactCGPU,0, + nContactOut , + maxNumBatches,numIter,&m_data->m_batchSizes); + } else + { + solveContactConstraint( + m_data->m_bodyBufferGPU, + m_data->m_inertiaBufferGPU, + m_data->m_contactCGPU,0, + nContactOut , + maxNumBatches,numIter,&m_data->m_batchSizes);//m_data->m_batchSizesGpu); + } + } + else + { + B3_PROFILE("Host solveContactConstraint"); + + m_data->m_solverGPU->solveContactConstraintHost(m_data->m_bodyBufferGPU, m_data->m_inertiaBufferGPU, m_data->m_contactCGPU,0, nContactOut ,maxNumBatches,&m_data->m_batchSizes); + } + + + } + + +#if 0 + if (0) + { + B3_PROFILE("read body velocities back to CPU"); + //read body updated linear/angular velocities back to CPU + m_data->m_bodyBufferGPU->read( + m_data->m_bodyBufferCPU->m_ptr,numOfConvexRBodies); + adl::DeviceUtils::waitForCompletion( m_data->m_deviceCL ); + } +#endif + + } + +} + + +void b3GpuPgsContactSolver::batchContacts( b3OpenCLArray* contacts, int nContacts, b3OpenCLArray* n, b3OpenCLArray* offsets, int staticIdx ) +{ +} + + + + + + + + + + + +b3AlignedObjectArray idxBuffer; +b3AlignedObjectArray sortData; +b3AlignedObjectArray old; + + +inline int b3GpuPgsContactSolver::sortConstraintByBatch( b3Contact4* cs, int n, int simdWidth , int staticIdx, int numBodies) +{ + + B3_PROFILE("sortConstraintByBatch"); + int numIter = 0; + + sortData.resize(n); + idxBuffer.resize(n); + old.resize(n); + + unsigned int* idxSrc = &idxBuffer[0]; + unsigned int* idxDst = &idxBuffer[0]; + int nIdxSrc, nIdxDst; + + const int N_FLG = 256; + const int FLG_MASK = N_FLG-1; + unsigned int flg[N_FLG/32]; +#if defined(_DEBUG) + for(int i=0; i bodyUsed2; + +inline int b3GpuPgsContactSolver::sortConstraintByBatch2( b3Contact4* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies) +{ + + B3_PROFILE("sortConstraintByBatch2"); + + + + bodyUsed2.resize(2*simdWidth); + + for (int q=0;q<2*simdWidth;q++) + bodyUsed2[q]=0; + + int curBodyUsed = 0; + + int numIter = 0; + + m_data->m_sortData.resize(numConstraints); + m_data->m_idxBuffer.resize(numConstraints); + m_data->m_old.resize(numConstraints); + + unsigned int* idxSrc = &m_data->m_idxBuffer[0]; + +#if defined(_DEBUG) + for(int i=0; im_sortData[idx].m_key = batchIdx; + m_data->m_sortData[idx].m_value = idx; + + if (i!=numValidConstraints) + { + b3Swap(idxSrc[i], idxSrc[numValidConstraints]); + } + + numValidConstraints++; + { + nCurrentBatch++; + if( nCurrentBatch == simdWidth ) + { + nCurrentBatch = 0; + for(int i=0; im_sortData.quickSort(sortfnc); + } + + { + B3_PROFILE("reorder"); + // reorder + + memcpy( &m_data->m_old[0], cs, sizeof(b3Contact4)*numConstraints); + + for(int i=0; im_sortData[idxSrc[i]].m_value == idxSrc[i]); + int idx = m_data->m_sortData[idxSrc[i]].m_value; + cs[i] = m_data->m_old[idx]; + } + } + +#if defined(_DEBUG) + // debugPrintf( "nBatches: %d\n", batchIdx ); + for(int i=0; i bodyUsed; +b3AlignedObjectArray curUsed; + + +inline int b3GpuPgsContactSolver::sortConstraintByBatch3( b3Contact4* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies, int* batchSizes) +{ + + B3_PROFILE("sortConstraintByBatch3"); + + static int maxSwaps = 0; + int numSwaps = 0; + + curUsed.resize(2*simdWidth); + + static int maxNumConstraints = 0; + if (maxNumConstraintsm_sortData.resize(0); + m_data->m_idxBuffer.resize(0); + m_data->m_old.resize(0); + + +#if defined(_DEBUG) + for(int i=0; i=B3_MAX_NUM_BATCHES) + { + b3Error("batchIdx>=B3_MAX_NUM_BATCHES"); + b3Assert(0); + break; + } + + batchSizes[batchIdx] += nCurrentBatch; + + batchIdx ++; + + } + } + +#if defined(_DEBUG) + // debugPrintf( "nBatches: %d\n", batchIdx ); + for(int i=0; i* contacts, int nContacts, b3OpenCLArray* n, b3OpenCLArray* offsets, int staticIdx ); + + inline int sortConstraintByBatch( b3Contact4* cs, int n, int simdWidth , int staticIdx, int numBodies); + inline int sortConstraintByBatch2( b3Contact4* cs, int n, int simdWidth , int staticIdx, int numBodies); + inline int sortConstraintByBatch3( b3Contact4* cs, int n, int simdWidth , int staticIdx, int numBodies, int* batchSizes); + + + + void solveContactConstraintBatchSizes( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, + b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches, int numIterations, const b3AlignedObjectArray* batchSizes);//const b3OpenCLArray* gpuBatchSizes); + + void solveContactConstraint( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* shapeBuf, + b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches, int numIterations, const b3AlignedObjectArray* batchSizes);//const b3OpenCLArray* gpuBatchSizes); + +public: + + b3GpuPgsContactSolver(cl_context ctx,cl_device_id device, cl_command_queue q,int pairCapacity); + virtual ~b3GpuPgsContactSolver(); + + void solveContacts(int numBodies, cl_mem bodyBuf, cl_mem inertiaBuf, int numContacts, cl_mem contactBuf, const struct b3Config& config, int static0Index); + +}; + +#endif //B3_GPU_BATCHING_PGS_SOLVER_H + diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp new file mode 100644 index 000000000000..783e4430604f --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp @@ -0,0 +1,708 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + +#include "b3GpuRigidBodyPipeline.h" +#include "b3GpuRigidBodyPipelineInternalData.h" +#include "kernels/integrateKernel.h" +#include "kernels/updateAabbsKernel.h" + +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "b3GpuNarrowPhase.h" +#include "Bullet3Geometry/b3AabbUtil.h" +#include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h" +#include "Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" +#include "Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h" +#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h" + +//#define TEST_OTHER_GPU_SOLVER + +#define B3_RIGIDBODY_INTEGRATE_PATH "src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl" +#define B3_RIGIDBODY_UPDATEAABB_PATH "src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl" + +bool useBullet2CpuSolver = true; + +//choice of contact solver +bool gUseJacobi = false; +bool gUseDbvt = false; +bool gDumpContactStats = false; +bool gCalcWorldSpaceAabbOnCpu = false; +bool gUseCalculateOverlappingPairsHost = false; +bool gIntegrateOnCpu = false; +bool gClearPairsOnGpu = true; + +#define TEST_OTHER_GPU_SOLVER 1 +#ifdef TEST_OTHER_GPU_SOLVER +#include "b3GpuJacobiContactSolver.h" +#endif //TEST_OTHER_GPU_SOLVER + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" +#include "Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h" + +#include "b3GpuPgsContactSolver.h" +#include "b3Solver.h" + +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" +#include "Bullet3OpenCL/Raycast/b3GpuRaycast.h" + + +#include "Bullet3Dynamics/shared/b3IntegrateTransforms.h" +#include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h" + +b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue q,class b3GpuNarrowPhase* narrowphase, class b3GpuBroadphaseInterface* broadphaseSap , struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config) +{ + m_data = new b3GpuRigidBodyPipelineInternalData; + m_data->m_constraintUid=0; + m_data->m_config = config; + m_data->m_context = ctx; + m_data->m_device = device; + m_data->m_queue = q; + + m_data->m_solver = new b3PgsJacobiSolver(true);//new b3PgsJacobiSolver(true); + m_data->m_gpuSolver = new b3GpuPgsConstraintSolver(ctx,device,q,true);//new b3PgsJacobiSolver(true); + + m_data->m_allAabbsGPU = new b3OpenCLArray(ctx,q,config.m_maxConvexBodies); + m_data->m_overlappingPairsGPU = new b3OpenCLArray(ctx,q,config.m_maxBroadphasePairs); + + m_data->m_gpuConstraints = new b3OpenCLArray(ctx,q); +#ifdef TEST_OTHER_GPU_SOLVER + m_data->m_solver3 = new b3GpuJacobiContactSolver(ctx,device,q,config.m_maxBroadphasePairs); +#endif // TEST_OTHER_GPU_SOLVER + + m_data->m_solver2 = new b3GpuPgsContactSolver(ctx,device,q,config.m_maxBroadphasePairs); + + m_data->m_raycaster = new b3GpuRaycast(ctx,device,q); + + + m_data->m_broadphaseDbvt = broadphaseDbvt; + m_data->m_broadphaseSap = broadphaseSap; + m_data->m_narrowphase = narrowphase; + m_data->m_gravity.setValue(0.f,-9.8f,0.f); + + cl_int errNum=0; + + { + cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_data->m_context,m_data->m_device,integrateKernelCL,&errNum,"",B3_RIGIDBODY_INTEGRATE_PATH); + b3Assert(errNum==CL_SUCCESS); + m_data->m_integrateTransformsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,integrateKernelCL, "integrateTransformsKernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + clReleaseProgram(prog); + } + { + cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_data->m_context,m_data->m_device,updateAabbsKernelCL,&errNum,"",B3_RIGIDBODY_UPDATEAABB_PATH); + b3Assert(errNum==CL_SUCCESS); + m_data->m_updateAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,updateAabbsKernelCL, "initializeGpuAabbsFull",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + + + m_data->m_clearOverlappingPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,updateAabbsKernelCL, "clearOverlappingPairsKernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + + clReleaseProgram(prog); + } + + +} + +b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline() +{ + if (m_data->m_integrateTransformsKernel) + clReleaseKernel(m_data->m_integrateTransformsKernel); + + if (m_data->m_updateAabbsKernel) + clReleaseKernel(m_data->m_updateAabbsKernel); + + if (m_data->m_clearOverlappingPairsKernel) + clReleaseKernel(m_data->m_clearOverlappingPairsKernel); + delete m_data->m_raycaster; + delete m_data->m_solver; + delete m_data->m_allAabbsGPU; + delete m_data->m_gpuConstraints; + delete m_data->m_overlappingPairsGPU; + +#ifdef TEST_OTHER_GPU_SOLVER + delete m_data->m_solver3; +#endif //TEST_OTHER_GPU_SOLVER + + delete m_data->m_solver2; + + + delete m_data; +} + +void b3GpuRigidBodyPipeline::reset() +{ + m_data->m_gpuConstraints->resize(0); + m_data->m_cpuConstraints.resize(0); + m_data->m_allAabbsGPU->resize(0); + m_data->m_allAabbsCPU.resize(0); +} + +void b3GpuRigidBodyPipeline::addConstraint(b3TypedConstraint* constraint) +{ + m_data->m_joints.push_back(constraint); +} + +void b3GpuRigidBodyPipeline::removeConstraint(b3TypedConstraint* constraint) +{ + m_data->m_joints.remove(constraint); +} + + + +void b3GpuRigidBodyPipeline::removeConstraintByUid(int uid) +{ + m_data->m_gpuSolver->recomputeBatches(); + //slow linear search + m_data->m_gpuConstraints->copyToHost(m_data->m_cpuConstraints); + //remove + for (int i=0;im_cpuConstraints.size();i++) + { + if (m_data->m_cpuConstraints[i].m_uid == uid) + { + //m_data->m_cpuConstraints.remove(m_data->m_cpuConstraints[i]); + m_data->m_cpuConstraints.swap(i,m_data->m_cpuConstraints.size()-1); + m_data->m_cpuConstraints.pop_back(); + + break; + } + } + + if (m_data->m_cpuConstraints.size()) + { + m_data->m_gpuConstraints->copyFromHost(m_data->m_cpuConstraints); + } else + { + m_data->m_gpuConstraints->resize(0); + } + +} +int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold) +{ + m_data->m_gpuSolver->recomputeBatches(); + b3GpuGenericConstraint c; + c.m_uid = m_data->m_constraintUid; + m_data->m_constraintUid++; + c.m_flags = B3_CONSTRAINT_FLAG_ENABLED; + c.m_rbA = bodyA; + c.m_rbB = bodyB; + c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]); + c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]); + c.m_breakingImpulseThreshold = breakingThreshold; + c.m_constraintType = B3_GPU_POINT2POINT_CONSTRAINT_TYPE; + m_data->m_cpuConstraints.push_back(c); + return c.m_uid; +} +int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB,float breakingThreshold) +{ + m_data->m_gpuSolver->recomputeBatches(); + b3GpuGenericConstraint c; + c.m_uid = m_data->m_constraintUid; + m_data->m_constraintUid++; + c.m_flags = B3_CONSTRAINT_FLAG_ENABLED; + c.m_rbA = bodyA; + c.m_rbB = bodyB; + c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]); + c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]); + c.m_relTargetAB.setValue(relTargetAB[0],relTargetAB[1],relTargetAB[2],relTargetAB[3]); + c.m_breakingImpulseThreshold = breakingThreshold; + c.m_constraintType = B3_GPU_FIXED_CONSTRAINT_TYPE; + + m_data->m_cpuConstraints.push_back(c); + return c.m_uid; +} + + +void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) +{ + + //update worldspace AABBs from local AABB/worldtransform + { + B3_PROFILE("setupGpuAabbs"); + setupGpuAabbsFull(); + } + + int numPairs =0; + + //compute overlapping pairs + { + + if (gUseDbvt) + { + { + B3_PROFILE("setAabb"); + m_data->m_allAabbsGPU->copyToHost(m_data->m_allAabbsCPU); + for (int i=0;im_allAabbsCPU.size();i++) + { + b3Vector3 aabbMin=b3MakeVector3(m_data->m_allAabbsCPU[i].m_min[0],m_data->m_allAabbsCPU[i].m_min[1],m_data->m_allAabbsCPU[i].m_min[2]); + b3Vector3 aabbMax=b3MakeVector3(m_data->m_allAabbsCPU[i].m_max[0],m_data->m_allAabbsCPU[i].m_max[1],m_data->m_allAabbsCPU[i].m_max[2]); + m_data->m_broadphaseDbvt->setAabb(i,aabbMin,aabbMax,0); + } + } + + { + B3_PROFILE("calculateOverlappingPairs"); + m_data->m_broadphaseDbvt->calculateOverlappingPairs(); + } + numPairs = m_data->m_broadphaseDbvt->getOverlappingPairCache()->getNumOverlappingPairs(); + + } else + { + if (gUseCalculateOverlappingPairsHost) + { + m_data->m_broadphaseSap->calculateOverlappingPairsHost(m_data->m_config.m_maxBroadphasePairs); + } else + { + m_data->m_broadphaseSap->calculateOverlappingPairs(m_data->m_config.m_maxBroadphasePairs); + } + numPairs = m_data->m_broadphaseSap->getNumOverlap(); + } + } + + //compute contact points +// printf("numPairs=%d\n",numPairs); + + int numContacts = 0; + + + int numBodies = m_data->m_narrowphase->getNumRigidBodies(); + + if (numPairs) + { + cl_mem pairs =0; + cl_mem aabbsWS =0; + if (gUseDbvt) + { + B3_PROFILE("m_overlappingPairsGPU->copyFromHost"); + m_data->m_overlappingPairsGPU->copyFromHost(m_data->m_broadphaseDbvt->getOverlappingPairCache()->getOverlappingPairArray()); + pairs = m_data->m_overlappingPairsGPU->getBufferCL(); + aabbsWS = m_data->m_allAabbsGPU->getBufferCL(); + } else + { + pairs = m_data->m_broadphaseSap->getOverlappingPairBuffer(); + aabbsWS = m_data->m_broadphaseSap->getAabbBufferWS(); + } + + m_data->m_overlappingPairsGPU->resize(numPairs); + + //mark the contacts for each pair as 'unused' + if (numPairs) + { + b3OpenCLArray gpuPairs(this->m_data->m_context,m_data->m_queue); + gpuPairs.setFromOpenCLBuffer(pairs,numPairs); + + if (gClearPairsOnGpu) + { + + + //b3AlignedObjectArray hostPairs;//just for debugging + //gpuPairs.copyToHost(hostPairs); + + b3LauncherCL launcher(m_data->m_queue,m_data->m_clearOverlappingPairsKernel,"clearOverlappingPairsKernel"); + launcher.setBuffer(pairs); + launcher.setConst(numPairs); + launcher.launch1D(numPairs); + + + //gpuPairs.copyToHost(hostPairs); + + + } else + { + b3AlignedObjectArray hostPairs; + gpuPairs.copyToHost(hostPairs); + + for (int i=0;im_narrowphase->computeContacts(pairs,numPairs,aabbsWS,numBodies); + numContacts = m_data->m_narrowphase->getNumContactsGpu(); + + if (gUseDbvt) + { + ///store the cached information (contact locations in the 'z' component) + B3_PROFILE("m_overlappingPairsGPU->copyToHost"); + m_data->m_overlappingPairsGPU->copyToHost(m_data->m_broadphaseDbvt->getOverlappingPairCache()->getOverlappingPairArray()); + } + if (gDumpContactStats && numContacts) + { + m_data->m_narrowphase->getContactsGpu(); + + printf("numContacts = %d\n", numContacts); + + int totalPoints = 0; + const b3Contact4* contacts = m_data->m_narrowphase->getContactsCPU(); + + for (int i=0;igetNPoints(); + } + printf("totalPoints=%d\n",totalPoints); + + } + } + + + //convert contact points to contact constraints + + //solve constraints + + b3OpenCLArray gpuBodies(m_data->m_context,m_data->m_queue,0,true); + gpuBodies.setFromOpenCLBuffer(m_data->m_narrowphase->getBodiesGpu(),m_data->m_narrowphase->getNumRigidBodies()); + b3OpenCLArray gpuInertias(m_data->m_context,m_data->m_queue,0,true); + gpuInertias.setFromOpenCLBuffer(m_data->m_narrowphase->getBodyInertiasGpu(),m_data->m_narrowphase->getNumRigidBodies()); + b3OpenCLArray gpuContacts(m_data->m_context,m_data->m_queue,0,true); + gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(),m_data->m_narrowphase->getNumContactsGpu()); + + int numJoints = m_data->m_joints.size() ? m_data->m_joints.size() : m_data->m_cpuConstraints.size(); + if (useBullet2CpuSolver && numJoints) + { + + // b3AlignedObjectArray hostContacts; + //gpuContacts.copyToHost(hostContacts); + { + bool useGpu = m_data->m_joints.size()==0; + +// b3Contact4* contacts = numContacts? &hostContacts[0]: 0; + //m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,contacts,numJoints, joints); + if (useGpu) + { + m_data->m_gpuSolver->solveJoints(m_data->m_narrowphase->getNumRigidBodies(),&gpuBodies,&gpuInertias,numJoints, m_data->m_gpuConstraints); + } else + { + b3AlignedObjectArray hostBodies; + gpuBodies.copyToHost(hostBodies); + b3AlignedObjectArray hostInertias; + gpuInertias.copyToHost(hostInertias); + + b3TypedConstraint** joints = numJoints? &m_data->m_joints[0] : 0; + m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumRigidBodies(),&hostBodies[0],&hostInertias[0],0,0,numJoints, joints); + gpuBodies.copyFromHost(hostBodies); + } + } + } + + if (numContacts) + { + +#ifdef TEST_OTHER_GPU_SOLVER + + if (gUseJacobi) + { + bool useGpu = true; + if (useGpu) + { + + bool forceHost = false; + if (forceHost) + { + b3AlignedObjectArray hostBodies; + b3AlignedObjectArray hostInertias; + b3AlignedObjectArray hostContacts; + + { + B3_PROFILE("copyToHost"); + gpuBodies.copyToHost(hostBodies); + gpuInertias.copyToHost(hostInertias); + gpuContacts.copyToHost(hostContacts); + } + + { + b3JacobiSolverInfo solverInfo; + m_data->m_solver3->solveGroupHost(&hostBodies[0], &hostInertias[0], hostBodies.size(),&hostContacts[0],hostContacts.size(),solverInfo); + + + } + { + B3_PROFILE("copyFromHost"); + gpuBodies.copyFromHost(hostBodies); + } + } else + + + { + int static0Index = m_data->m_narrowphase->getStatic0Index(); + b3JacobiSolverInfo solverInfo; + //m_data->m_solver3->solveContacts( >solveGroup(&gpuBodies, &gpuInertias, &gpuContacts,solverInfo); + //m_data->m_solver3->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,&hostContacts[0]); + m_data->m_solver3->solveContacts(numBodies, gpuBodies.getBufferCL(),gpuInertias.getBufferCL(),numContacts, gpuContacts.getBufferCL(),m_data->m_config, static0Index); + } + } else + { + b3AlignedObjectArray hostBodies; + gpuBodies.copyToHost(hostBodies); + b3AlignedObjectArray hostInertias; + gpuInertias.copyToHost(hostInertias); + b3AlignedObjectArray hostContacts; + gpuContacts.copyToHost(hostContacts); + { + //m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,&hostContacts[0]); + } + gpuBodies.copyFromHost(hostBodies); + } + + } else +#endif //TEST_OTHER_GPU_SOLVER + { + + int static0Index = m_data->m_narrowphase->getStatic0Index(); + m_data->m_solver2->solveContacts(numBodies, gpuBodies.getBufferCL(),gpuInertias.getBufferCL(),numContacts, gpuContacts.getBufferCL(),m_data->m_config, static0Index); + + //m_data->m_solver4->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(), gpuBodies.getBufferCL(), gpuInertias.getBufferCL(), numContacts, gpuContacts.getBufferCL()); + + + /*m_data->m_solver3->solveContactConstraintHost( + (b3OpenCLArray*)&gpuBodies, + (b3OpenCLArray*)&gpuInertias, + (b3OpenCLArray*) &gpuContacts, + 0,numContacts,256); + */ + } + } + + integrate(deltaTime); + +} + + +void b3GpuRigidBodyPipeline::integrate(float timeStep) +{ + //integrate + int numBodies = m_data->m_narrowphase->getNumRigidBodies(); + float angularDamp = 0.99f; + + if (gIntegrateOnCpu) + { + if(numBodies) + { + b3GpuNarrowPhaseInternalData* npData = m_data->m_narrowphase->getInternalData(); + npData->m_bodyBufferGPU->copyToHost(*npData->m_bodyBufferCPU); + + b3RigidBodyData_t* bodies = &npData->m_bodyBufferCPU->at(0); + + for (int nodeID=0;nodeIDm_gravity); + } + npData->m_bodyBufferGPU->copyFromHost(*npData->m_bodyBufferCPU); + } + } else + { + b3LauncherCL launcher(m_data->m_queue,m_data->m_integrateTransformsKernel,"m_integrateTransformsKernel"); + launcher.setBuffer(m_data->m_narrowphase->getBodiesGpu()); + + launcher.setConst(numBodies); + launcher.setConst(timeStep); + launcher.setConst(angularDamp); + launcher.setConst(m_data->m_gravity); + launcher.launch1D(numBodies); + } +} + + + + +void b3GpuRigidBodyPipeline::setupGpuAabbsFull() +{ + cl_int ciErrNum=0; + + int numBodies = m_data->m_narrowphase->getNumRigidBodies(); + if (!numBodies) + return; + + if (gCalcWorldSpaceAabbOnCpu) + { + + if (numBodies) + { + if (gUseDbvt) + { + m_data->m_allAabbsCPU.resize(numBodies); + m_data->m_narrowphase->readbackAllBodiesToCpu(); + for (int i=0;im_narrowphase->getBodiesCpu(), m_data->m_narrowphase->getCollidablesCpu(), m_data->m_narrowphase->getLocalSpaceAabbsCpu(),&m_data->m_allAabbsCPU[0]); + } + m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); + } else + { + m_data->m_broadphaseSap->getAllAabbsCPU().resize(numBodies); + m_data->m_narrowphase->readbackAllBodiesToCpu(); + for (int i=0;im_narrowphase->getBodiesCpu(), m_data->m_narrowphase->getCollidablesCpu(), m_data->m_narrowphase->getLocalSpaceAabbsCpu(),&m_data->m_broadphaseSap->getAllAabbsCPU()[0]); + } + m_data->m_broadphaseSap->getAllAabbsGPU().copyFromHost(m_data->m_broadphaseSap->getAllAabbsCPU()); + //m_data->m_broadphaseSap->writeAabbsToGpu(); + } + } + } else + { + //__kernel void initializeGpuAabbsFull( const int numNodes, __global Body* gBodies,__global Collidable* collidables, __global b3AABBCL* plocalShapeAABB, __global b3AABBCL* pAABB) + b3LauncherCL launcher(m_data->m_queue,m_data->m_updateAabbsKernel,"m_updateAabbsKernel"); + launcher.setConst(numBodies); + cl_mem bodies = m_data->m_narrowphase->getBodiesGpu(); + launcher.setBuffer(bodies); + cl_mem collidables = m_data->m_narrowphase->getCollidablesGpu(); + launcher.setBuffer(collidables); + cl_mem localAabbs = m_data->m_narrowphase->getAabbLocalSpaceBufferGpu(); + launcher.setBuffer(localAabbs); + + cl_mem worldAabbs =0; + if (gUseDbvt) + { + worldAabbs = m_data->m_allAabbsGPU->getBufferCL(); + } else + { + worldAabbs = m_data->m_broadphaseSap->getAabbBufferWS(); + } + launcher.setBuffer(worldAabbs); + launcher.launch1D(numBodies); + + oclCHECKERROR(ciErrNum, CL_SUCCESS); + } + + /* + b3AlignedObjectArray aabbs; + m_data->m_broadphaseSap->m_allAabbsGPU.copyToHost(aabbs); + + printf("numAabbs = %d\n", aabbs.size()); + + for (int i=0;im_narrowphase->getBodiesGpu(); +} + +int b3GpuRigidBodyPipeline::getNumBodies() const +{ + return m_data->m_narrowphase->getNumRigidBodies(); +} + +void b3GpuRigidBodyPipeline::setGravity(const float* grav) +{ + m_data->m_gravity.setValue(grav[0],grav[1],grav[2]); +} + +void b3GpuRigidBodyPipeline::copyConstraintsToHost() +{ + m_data->m_gpuConstraints->copyToHost(m_data->m_cpuConstraints); +} + +void b3GpuRigidBodyPipeline::writeAllInstancesToGpu() +{ + m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); + m_data->m_gpuConstraints->copyFromHost(m_data->m_cpuConstraints); +} + + +int b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userIndex, bool writeInstanceToGpu) +{ + + b3Vector3 aabbMin=b3MakeVector3(0,0,0),aabbMax=b3MakeVector3(0,0,0); + + + if (collidableIndex>=0) + { + b3SapAabb localAabb = m_data->m_narrowphase->getLocalSpaceAabb(collidableIndex); + b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]); + b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]); + + b3Scalar margin = 0.01f; + b3Transform t; + t.setIdentity(); + t.setOrigin(b3MakeVector3(position[0],position[1],position[2])); + t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3])); + b3TransformAabb(localAabbMin,localAabbMax, margin,t,aabbMin,aabbMax); + } else + { + b3Error("registerPhysicsInstance using invalid collidableIndex\n"); + return -1; + } + + + bool writeToGpu = false; + int bodyIndex = m_data->m_narrowphase->getNumRigidBodies(); + bodyIndex = m_data->m_narrowphase->registerRigidBody(collidableIndex,mass,position,orientation,&aabbMin.getX(),&aabbMax.getX(),writeToGpu); + + if (bodyIndex>=0) + { + if (gUseDbvt) + { + m_data->m_broadphaseDbvt->createProxy(aabbMin,aabbMax,bodyIndex,0,1,1); + b3SapAabb aabb; + for (int i=0;i<3;i++) + { + aabb.m_min[i] = aabbMin[i]; + aabb.m_max[i] = aabbMax[i]; + aabb.m_minIndices[3] = bodyIndex; + } + m_data->m_allAabbsCPU.push_back(aabb); + if (writeInstanceToGpu) + { + m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); + } + } else + { + if (mass) + { + m_data->m_broadphaseSap->createProxy(aabbMin,aabbMax,bodyIndex,1,1);//m_dispatcher); + } else + { + m_data->m_broadphaseSap->createLargeProxy(aabbMin,aabbMax,bodyIndex,1,1);//m_dispatcher); + } + } + } + + /* + if (mass>0.f) + m_numDynamicPhysicsInstances++; + + m_numPhysicsInstances++; + */ + + return bodyIndex; +} + +void b3GpuRigidBodyPipeline::castRays(const b3AlignedObjectArray& rays, b3AlignedObjectArray& hitResults) +{ + this->m_data->m_raycaster->castRays(rays,hitResults, + getNumBodies(),this->m_data->m_narrowphase->getBodiesCpu(), + m_data->m_narrowphase->getNumCollidablesGpu(), m_data->m_narrowphase->getCollidablesCpu(), + m_data->m_narrowphase->getInternalData(), m_data->m_broadphaseSap); +} diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h new file mode 100644 index 000000000000..b4eac6841a11 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h @@ -0,0 +1,74 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + +#ifndef B3_GPU_RIGIDBODY_PIPELINE_H +#define B3_GPU_RIGIDBODY_PIPELINE_H + +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" + +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h" + +class b3GpuRigidBodyPipeline +{ +protected: + struct b3GpuRigidBodyPipelineInternalData* m_data; + + int allocateCollidable(); + +public: + + + b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue q , class b3GpuNarrowPhase* narrowphase, class b3GpuBroadphaseInterface* broadphaseSap, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config); + virtual ~b3GpuRigidBodyPipeline(); + + void stepSimulation(float deltaTime); + void integrate(float timeStep); + void setupGpuAabbsFull(); + + int registerConvexPolyhedron(class b3ConvexUtility* convex); + + //int registerConvexPolyhedron(const float* vertices, int strideInBytes, int numVertices, const float* scaling); + //int registerSphereShape(float radius); + //int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant); + + //int registerConcaveMesh(b3AlignedObjectArray* vertices, b3AlignedObjectArray* indices, const float* scaling); + //int registerCompoundShape(b3AlignedObjectArray* childShapes); + + + int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData, bool writeInstanceToGpu); + //if you passed "writeInstanceToGpu" false in the registerPhysicsInstance method (for performance) you need to call writeAllInstancesToGpu after all instances are registered + void writeAllInstancesToGpu(); + void copyConstraintsToHost(); + void setGravity(const float* grav); + void reset(); + + int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold); + int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold); + void removeConstraintByUid(int uid); + + void addConstraint(class b3TypedConstraint* constraint); + void removeConstraint(b3TypedConstraint* constraint); + + void castRays(const b3AlignedObjectArray& rays, b3AlignedObjectArray& hitResults); + + cl_mem getBodyBuffer(); + + int getNumBodies() const; + +}; + +#endif //B3_GPU_RIGIDBODY_PIPELINE_H \ No newline at end of file diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h new file mode 100644 index 000000000000..5ac92f97d612 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h @@ -0,0 +1,73 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + +#ifndef B3_GPU_RIGIDBODY_PIPELINE_INTERNAL_DATA_H +#define B3_GPU_RIGIDBODY_PIPELINE_INTERNAL_DATA_H + +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" + + +#include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h" +#include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" + + + +#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h" +#include "Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h" + +struct b3GpuRigidBodyPipelineInternalData +{ + + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; + + cl_kernel m_integrateTransformsKernel; + cl_kernel m_updateAabbsKernel; + cl_kernel m_clearOverlappingPairsKernel; + + class b3PgsJacobiSolver* m_solver; + + class b3GpuPgsConstraintSolver* m_gpuSolver; + + class b3GpuPgsContactSolver* m_solver2; + class b3GpuJacobiContactSolver* m_solver3; + class b3GpuRaycast* m_raycaster; + + class b3GpuBroadphaseInterface* m_broadphaseSap; + + struct b3DynamicBvhBroadphase* m_broadphaseDbvt; + b3OpenCLArray* m_allAabbsGPU; + b3AlignedObjectArray m_allAabbsCPU; + b3OpenCLArray* m_overlappingPairsGPU; + + b3OpenCLArray* m_gpuConstraints; + b3AlignedObjectArray m_cpuConstraints; + + b3AlignedObjectArray m_joints; + int m_constraintUid; + class b3GpuNarrowPhase* m_narrowphase; + b3Vector3 m_gravity; + + b3Config m_config; +}; + +#endif //B3_GPU_RIGIDBODY_PIPELINE_INTERNAL_DATA_H + diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h new file mode 100644 index 000000000000..f2a61801ac9a --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h @@ -0,0 +1,228 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + + +#ifndef B3_GPU_SOLVER_BODY_H +#define B3_GPU_SOLVER_BODY_H + + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Matrix3x3.h" + +#include "Bullet3Common/b3AlignedAllocator.h" +#include "Bullet3Common/b3TransformUtil.h" + +///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision +#ifdef B3_USE_SSE +#define USE_SIMD 1 +#endif // + + + +///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. +B3_ATTRIBUTE_ALIGNED16 (struct) b3GpuSolverBody +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); +// b3Transform m_worldTransformUnused; + b3Vector3 m_deltaLinearVelocity; + b3Vector3 m_deltaAngularVelocity; + b3Vector3 m_angularFactor; + b3Vector3 m_linearFactor; + b3Vector3 m_invMass; + b3Vector3 m_pushVelocity; + b3Vector3 m_turnVelocity; + b3Vector3 m_linearVelocity; + b3Vector3 m_angularVelocity; + + union + { + void* m_originalBody; + int m_originalBodyIndex; + }; + + int padding[3]; + + /* + void setWorldTransform(const b3Transform& worldTransform) + { + m_worldTransform = worldTransform; + } + + const b3Transform& getWorldTransform() const + { + return m_worldTransform; + } + */ + B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const + { + if (m_originalBody) + velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + else + velocity.setValue(0,0,0); + } + + B3_FORCE_INLINE void getAngularVelocity(b3Vector3& angVel) const + { + if (m_originalBody) + angVel =m_angularVelocity+m_deltaAngularVelocity; + else + angVel.setValue(0,0,0); + } + + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude) + { + if (m_originalBody) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,b3Scalar impulseMagnitude) + { + if (m_originalBody) + { + m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + + + const b3Vector3& getDeltaLinearVelocity() const + { + return m_deltaLinearVelocity; + } + + const b3Vector3& getDeltaAngularVelocity() const + { + return m_deltaAngularVelocity; + } + + const b3Vector3& getPushVelocity() const + { + return m_pushVelocity; + } + + const b3Vector3& getTurnVelocity() const + { + return m_turnVelocity; + } + + + //////////////////////////////////////////////// + ///some internal methods, don't use them + + b3Vector3& internalGetDeltaLinearVelocity() + { + return m_deltaLinearVelocity; + } + + b3Vector3& internalGetDeltaAngularVelocity() + { + return m_deltaAngularVelocity; + } + + const b3Vector3& internalGetAngularFactor() const + { + return m_angularFactor; + } + + const b3Vector3& internalGetInvMass() const + { + return m_invMass; + } + + void internalSetInvMass(const b3Vector3& invMass) + { + m_invMass = invMass; + } + + b3Vector3& internalGetPushVelocity() + { + return m_pushVelocity; + } + + b3Vector3& internalGetTurnVelocity() + { + return m_turnVelocity; + } + + B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const + { + velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + } + + B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3& angVel) const + { + angVel = m_angularVelocity+m_deltaAngularVelocity; + } + + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude) + { + //if (m_originalBody) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + + + + void writebackVelocity() + { + //if (m_originalBody>=0) + { + m_linearVelocity +=m_deltaLinearVelocity; + m_angularVelocity += m_deltaAngularVelocity; + + //m_originalBody->setCompanionId(-1); + } + } + + + void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp) + { + (void) timeStep; + if (m_originalBody) + { + m_linearVelocity += m_deltaLinearVelocity; + m_angularVelocity += m_deltaAngularVelocity; + + //correct the position/orientation based on push/turn recovery + b3Transform newTransform; + if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0) + { + // b3Quaternion orn = m_worldTransform.getRotation(); +// b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform); +// m_worldTransform = newTransform; + } + //m_worldTransform.setRotation(orn); + //m_originalBody->setCompanionId(-1); + } + } + + + +}; + +#endif //B3_SOLVER_BODY_H + + diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h new file mode 100644 index 000000000000..60d235baab04 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h @@ -0,0 +1,82 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://github.com/erwincoumans/bullet3 + +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 B3_GPU_SOLVER_CONSTRAINT_H +#define B3_GPU_SOLVER_CONSTRAINT_H + + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Matrix3x3.h" +//#include "b3JacobianEntry.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +//#define NO_FRICTION_TANGENTIALS 1 + + + +///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. +B3_ATTRIBUTE_ALIGNED16 (struct) b3GpuSolverConstraint +{ + B3_DECLARE_ALIGNED_ALLOCATOR(); + + b3Vector3 m_relpos1CrossNormal; + b3Vector3 m_contactNormal; + + b3Vector3 m_relpos2CrossNormal; + //b3Vector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal + + b3Vector3 m_angularComponentA; + b3Vector3 m_angularComponentB; + + mutable b3Scalar m_appliedPushImpulse; + mutable b3Scalar m_appliedImpulse; + int m_padding1; + int m_padding2; + b3Scalar m_friction; + b3Scalar m_jacDiagABInv; + b3Scalar m_rhs; + b3Scalar m_cfm; + + b3Scalar m_lowerLimit; + b3Scalar m_upperLimit; + b3Scalar m_rhsPenetration; + union + { + void* m_originalContactPoint; + int m_originalConstraintIndex; + b3Scalar m_unusedPadding4; + }; + + int m_overrideNumSolverIterations; + int m_frictionIndex; + int m_solverBodyIdA; + int m_solverBodyIdB; + + + enum b3SolverConstraintType + { + B3_SOLVER_CONTACT_1D = 0, + B3_SOLVER_FRICTION_1D + }; +}; + +typedef b3AlignedObjectArray b3GpuConstraintArray; + + +#endif //B3_GPU_SOLVER_CONSTRAINT_H + + + diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.cpp b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.cpp new file mode 100644 index 000000000000..20bf6d47c5ef --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.cpp @@ -0,0 +1,1225 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, 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. +*/ +//Originally written by Takahiro Harada + + +#include "b3Solver.h" + +///useNewBatchingKernel is a rewritten kernel using just a single thread of the warp, for experiments +bool useNewBatchingKernel = true; +bool gConvertConstraintOnCpu = false; + +#define B3_SOLVER_SETUP_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl" +#define B3_SOLVER_SETUP2_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl" +#define B3_SOLVER_CONTACT_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl" +#define B3_SOLVER_FRICTION_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl" +#define B3_BATCHING_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl" +#define B3_BATCHING_NEW_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl" + +#include "Bullet3Dynamics/shared/b3ConvertConstraint4.h" + +#include "kernels/solverSetup.h" +#include "kernels/solverSetup2.h" + +#include "kernels/solveContact.h" +#include "kernels/solveFriction.h" + +#include "kernels/batchingKernels.h" +#include "kernels/batchingKernelsNew.h" + + +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" +#include "Bullet3Common/b3Vector3.h" + +struct SolverDebugInfo +{ + int m_valInt0; + int m_valInt1; + int m_valInt2; + int m_valInt3; + + int m_valInt4; + int m_valInt5; + int m_valInt6; + int m_valInt7; + + int m_valInt8; + int m_valInt9; + int m_valInt10; + int m_valInt11; + + int m_valInt12; + int m_valInt13; + int m_valInt14; + int m_valInt15; + + + float m_val0; + float m_val1; + float m_val2; + float m_val3; +}; + + + + +class SolverDeviceInl +{ +public: + struct ParallelSolveData + { + b3OpenCLArray* m_numConstraints; + b3OpenCLArray* m_offsets; + }; +}; + + + +b3Solver::b3Solver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity) + : + m_context(ctx), + m_device(device), + m_queue(queue), + m_batchSizes(ctx,queue), + m_nIterations(4) +{ + m_sort32 = new b3RadixSort32CL(ctx,device,queue); + m_scan = new b3PrefixScanCL(ctx,device,queue,B3_SOLVER_N_CELLS); + m_search = new b3BoundSearchCL(ctx,device,queue,B3_SOLVER_N_CELLS); + + const int sortSize = B3NEXTMULTIPLEOF( pairCapacity, 512 ); + + m_sortDataBuffer = new b3OpenCLArray(ctx,queue,sortSize); + m_contactBuffer2 = new b3OpenCLArray(ctx,queue); + + m_numConstraints = new b3OpenCLArray(ctx,queue,B3_SOLVER_N_CELLS ); + m_numConstraints->resize(B3_SOLVER_N_CELLS); + + m_offsets = new b3OpenCLArray( ctx,queue,B3_SOLVER_N_CELLS); + m_offsets->resize(B3_SOLVER_N_CELLS); + const char* additionalMacros = ""; +// const char* srcFileNameForCaching=""; + + + + cl_int pErrNum; + const char* batchKernelSource = batchingKernelsCL; + const char* batchKernelNewSource = batchingKernelsNewCL; + + const char* solverSetupSource = solverSetupCL; + const char* solverSetup2Source = solverSetup2CL; + const char* solveContactSource = solveContactCL; + const char* solveFrictionSource = solveFrictionCL; + + + + { + + cl_program solveContactProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveContactSource, &pErrNum,additionalMacros, B3_SOLVER_CONTACT_KERNEL_PATH); + b3Assert(solveContactProg); + + cl_program solveFrictionProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveFrictionSource, &pErrNum,additionalMacros, B3_SOLVER_FRICTION_KERNEL_PATH); + b3Assert(solveFrictionProg); + + cl_program solverSetup2Prog= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetup2Source, &pErrNum,additionalMacros, B3_SOLVER_SETUP2_KERNEL_PATH); + b3Assert(solverSetup2Prog); + + + cl_program solverSetupProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetupSource, &pErrNum,additionalMacros, B3_SOLVER_SETUP_KERNEL_PATH); + b3Assert(solverSetupProg); + + + m_solveFrictionKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveFrictionSource, "BatchSolveKernelFriction", &pErrNum, solveFrictionProg,additionalMacros ); + b3Assert(m_solveFrictionKernel); + + m_solveContactKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveContactSource, "BatchSolveKernelContact", &pErrNum, solveContactProg,additionalMacros ); + b3Assert(m_solveContactKernel); + + m_contactToConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetupSource, "ContactToConstraintKernel", &pErrNum, solverSetupProg,additionalMacros ); + b3Assert(m_contactToConstraintKernel); + + m_setSortDataKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetSortDataKernel", &pErrNum, solverSetup2Prog,additionalMacros ); + b3Assert(m_setSortDataKernel); + + m_reorderContactKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "ReorderContactKernel", &pErrNum, solverSetup2Prog,additionalMacros ); + b3Assert(m_reorderContactKernel); + + + m_copyConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "CopyConstraintKernel", &pErrNum, solverSetup2Prog,additionalMacros ); + b3Assert(m_copyConstraintKernel); + + } + + { + cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelSource, &pErrNum,additionalMacros, B3_BATCHING_PATH); + //cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, 0, &pErrNum,additionalMacros, B3_BATCHING_PATH,true); + b3Assert(batchingProg); + + m_batchingKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelSource, "CreateBatches", &pErrNum, batchingProg,additionalMacros ); + b3Assert(m_batchingKernel); + } + { + cl_program batchingNewProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelNewSource, &pErrNum,additionalMacros, B3_BATCHING_NEW_PATH); + b3Assert(batchingNewProg); + + m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesNew", &pErrNum, batchingNewProg,additionalMacros ); + //m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesBruteForce", &pErrNum, batchingNewProg,additionalMacros ); + b3Assert(m_batchingKernelNew); + } +} + +b3Solver::~b3Solver() +{ + delete m_offsets; + delete m_numConstraints; + delete m_sortDataBuffer; + delete m_contactBuffer2; + + delete m_sort32; + delete m_scan; + delete m_search; + + + clReleaseKernel(m_batchingKernel); + clReleaseKernel(m_batchingKernelNew); + + clReleaseKernel( m_solveContactKernel); + clReleaseKernel( m_solveFrictionKernel); + + clReleaseKernel( m_contactToConstraintKernel); + clReleaseKernel( m_setSortDataKernel); + clReleaseKernel( m_reorderContactKernel); + clReleaseKernel( m_copyConstraintKernel); + +} + + + + +template +static +__inline +void solveContact(b3GpuConstraint4& cs, + const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4]) +{ + + b3Vector3 dLinVelA; dLinVelA.setZero(); + b3Vector3 dAngVelA; dAngVelA.setZero(); + b3Vector3 dLinVelB; dLinVelB.setZero(); + b3Vector3 dAngVelB; dAngVelB.setZero(); + + for(int ic=0; ic<4; ic++) + { + // dont necessary because this makes change to 0 + if( cs.m_jacCoeffInv[ic] == 0.f ) continue; + + { + b3Vector3 angular0, angular1, linear; + b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA; + b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB; + setLinearAndAngular( (const b3Vector3 &)cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, &linear, &angular0, &angular1 ); + + float rambdaDt = calcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic]; + rambdaDt *= cs.m_jacCoeffInv[ic]; + + { + float prevSum = cs.m_appliedRambdaDt[ic]; + float updated = prevSum; + updated += rambdaDt; + updated = b3Max( updated, minRambdaDt[ic] ); + updated = b3Min( updated, maxRambdaDt[ic] ); + rambdaDt = updated - prevSum; + cs.m_appliedRambdaDt[ic] = updated; + } + + b3Vector3 linImp0 = invMassA*linear*rambdaDt; + b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; + b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; + b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; +#ifdef _WIN32 + b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp1.getX())); +#endif + if( JACOBI ) + { + dLinVelA += linImp0; + dAngVelA += angImp0; + dLinVelB += linImp1; + dAngVelB += angImp1; + } + else + { + linVelA += linImp0; + angVelA += angImp0; + linVelB += linImp1; + angVelB += angImp1; + } + } + } + + if( JACOBI ) + { + linVelA += dLinVelA; + angVelA += dAngVelA; + linVelB += dLinVelB; + angVelB += dAngVelB; + } + +} + + + + + + static + __inline + void solveFriction(b3GpuConstraint4& cs, + const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4]) + { + + if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return; + const b3Vector3& center = (const b3Vector3&)cs.m_center; + + b3Vector3 n = -(const b3Vector3&)cs.m_linear; + + b3Vector3 tangent[2]; +#if 1 + b3PlaneSpace1 (n, tangent[0],tangent[1]); +#else + b3Vector3 r = cs.m_worldPos[0]-center; + tangent[0] = cross3( n, r ); + tangent[1] = cross3( tangent[0], n ); + tangent[0] = normalize3( tangent[0] ); + tangent[1] = normalize3( tangent[1] ); +#endif + + b3Vector3 angular0, angular1, linear; + b3Vector3 r0 = center - posA; + b3Vector3 r1 = center - posB; + for(int i=0; i<2; i++) + { + setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 ); + float rambdaDt = calcRelVel(linear, -linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB ); + rambdaDt *= cs.m_fJacCoeffInv[i]; + + { + float prevSum = cs.m_fAppliedRambdaDt[i]; + float updated = prevSum; + updated += rambdaDt; + updated = b3Max( updated, minRambdaDt[i] ); + updated = b3Min( updated, maxRambdaDt[i] ); + rambdaDt = updated - prevSum; + cs.m_fAppliedRambdaDt[i] = updated; + } + + b3Vector3 linImp0 = invMassA*linear*rambdaDt; + b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; + b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; + b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; +#ifdef _WIN32 + b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp1.getX())); +#endif + linVelA += linImp0; + angVelA += angImp0; + linVelB += linImp1; + angVelB += angImp1; + } + + { // angular damping for point constraint + b3Vector3 ab = ( posB - posA ).normalized(); + b3Vector3 ac = ( center - posA ).normalized(); + if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) + { + float angNA = b3Dot( n, angVelA ); + float angNB = b3Dot( n, angVelB ); + + angVelA -= (angNA*0.1f)*n; + angVelB -= (angNB*0.1f)*n; + } + } + + } +/* + b3AlignedObjectArray& m_bodies; + b3AlignedObjectArray& m_shapes; + b3AlignedObjectArray& m_constraints; + b3AlignedObjectArray* m_batchSizes; + int m_cellIndex; + int m_curWgidx; + int m_start; + int m_nConstraints; + bool m_solveFriction; + int m_maxNumBatches; + */ + +struct SolveTask// : public ThreadPool::Task +{ + SolveTask(b3AlignedObjectArray& bodies, b3AlignedObjectArray& shapes, b3AlignedObjectArray& constraints, + int start, int nConstraints,int maxNumBatches,b3AlignedObjectArray* wgUsedBodies, int curWgidx, b3AlignedObjectArray* batchSizes, int cellIndex) + : m_bodies( bodies ), m_shapes( shapes ), + m_constraints( constraints ), + m_batchSizes(batchSizes), + m_cellIndex(cellIndex), + m_curWgidx(curWgidx), + m_start( start ), + m_nConstraints( nConstraints ), + m_solveFriction( true ), + m_maxNumBatches(maxNumBatches) + {} + + unsigned short int getType(){ return 0; } + + void run(int tIdx) + { + int offset = 0; + for (int ii=0;iiat(m_cellIndex*B3_MAX_NUM_BATCHES+ii); + if (!numInBatch) + break; + + for (int jj=0;jj( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt ); + } + else + { + float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; + float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; + float sum = 0; + for(int j=0; j<4; j++) + { + sum +=m_constraints[i].m_appliedRambdaDt[j]; + } + frictionCoeff = 0.7f; + for(int j=0; j<4; j++) + { + maxRambdaDt[j] = frictionCoeff*sum; + minRambdaDt[j] = -maxRambdaDt[j]; + } + solveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt ); + + } + } + offset+=numInBatch; + + + } +/* for (int bb=0;bb=0; ic--) + for(int ic=0; ic( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt ); + } + else + { + float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; + float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; + float sum = 0; + for(int j=0; j<4; j++) + { + sum +=m_constraints[i].m_appliedRambdaDt[j]; + } + frictionCoeff = 0.7f; + for(int j=0; j<4; j++) + { + maxRambdaDt[j] = frictionCoeff*sum; + minRambdaDt[j] = -maxRambdaDt[j]; + } + solveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt ); + + } + } + } + */ + + + + } + + b3AlignedObjectArray& m_bodies; + b3AlignedObjectArray& m_shapes; + b3AlignedObjectArray& m_constraints; + b3AlignedObjectArray* m_batchSizes; + int m_cellIndex; + int m_curWgidx; + int m_start; + int m_nConstraints; + bool m_solveFriction; + int m_maxNumBatches; +}; + + +void b3Solver::solveContactConstraintHost( b3OpenCLArray* bodyBuf, b3OpenCLArray* shapeBuf, + b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches,b3AlignedObjectArray* batchSizes) +{ + +#if 0 + { + int nSplitX = B3_SOLVER_N_SPLIT_X; + int nSplitY = B3_SOLVER_N_SPLIT_Y; + int numWorkgroups = B3_SOLVER_N_CELLS/B3_SOLVER_N_BATCHES; + for (int z=0;z<4;z++) + { + for (int y=0;y<4;y++) + { + for (int x=0;x<4;x++) + { + int newIndex = (x+y*nSplitX+z*nSplitX*nSplitY); + // printf("newIndex=%d\n",newIndex); + + int zIdx = newIndex/(nSplitX*nSplitY); + int remain = newIndex%(nSplitX*nSplitY); + int yIdx = remain/nSplitX; + int xIdx = remain%nSplitX; + // printf("newIndex=%d\n",newIndex); + } + } + } + + //for (int wgIdx=numWorkgroups-1;wgIdx>=0;wgIdx--) + for (int cellBatch=0;cellBatch>2); + int remain= (wgIdx%((nSplitX*nSplitY)/4)); + int yIdx = (remain/(nSplitX/2))*2 + ((cellBatch&2)>>1); + int xIdx = (remain%(nSplitX/2))*2 + (cellBatch&1); + + /*int zIdx = newIndex/(nSplitX*nSplitY); + int remain = newIndex%(nSplitX*nSplitY); + int yIdx = remain/nSplitX; + int xIdx = remain%nSplitX; + */ + int cellIdx = xIdx+yIdx*nSplitX+zIdx*(nSplitX*nSplitY); + // printf("wgIdx %d: xIdx=%d, yIdx=%d, zIdx=%d, cellIdx=%d, cell Batch %d\n",wgIdx,xIdx,yIdx,zIdx,cellIdx,cellBatch); + } + } + } +#endif + + b3AlignedObjectArray bodyNative; + bodyBuf->copyToHost(bodyNative); + b3AlignedObjectArray shapeNative; + shapeBuf->copyToHost(shapeNative); + b3AlignedObjectArray constraintNative; + constraint->copyToHost(constraintNative); + + b3AlignedObjectArray numConstraintsHost; + m_numConstraints->copyToHost(numConstraintsHost); + + //printf("------------------------\n"); + b3AlignedObjectArray offsetsHost; + m_offsets->copyToHost(offsetsHost); + static int frame=0; + bool useBatches=true; + if (useBatches) + { + for(int iter=0; iter usedBodies[B3_SOLVER_N_CELLS]; + for (int i=0;i=0;wgIdx--) + for (int wgIdx=0;wgIdx>2); + int remain= (wgIdx%((nSplitX*nSplitY)/4)); + int yIdx = (remain/(nSplitX/2))*2 + ((cellBatch&2)>>1); + int xIdx = (remain%(nSplitX/2))*2 + (cellBatch&1); + int cellIdx = xIdx+yIdx*nSplitX+zIdx*(nSplitX*nSplitY); + + + if( numConstraintsHost[cellIdx] == 0 ) + continue; + + //printf("wgIdx %d: xIdx=%d, yIdx=%d, zIdx=%d, cellIdx=%d, cell Batch %d\n",wgIdx,xIdx,yIdx,zIdx,cellIdx,cellBatch); + //printf("cell %d has %d constraints\n", cellIdx,numConstraintsHost[cellIdx]); + if (zIdx) + { + //printf("?\n"); + } + + if (iter==0) + { + //printf("frame=%d, Cell xIdx=%x, yIdx=%d ",frame, xIdx,yIdx); + //printf("cellBatch=%d, wgIdx=%d, #constraints in cell=%d\n",cellBatch,wgIdx,numConstraintsHost[cellIdx]); + } + const int start = offsetsHost[cellIdx]; + int numConstraintsInCell = numConstraintsHost[cellIdx]; + // const int end = start + numConstraintsInCell; + + SolveTask task( bodyNative, shapeNative, constraintNative, start, numConstraintsInCell ,maxNumBatches,usedBodies,wgIdx,batchSizes,cellIdx); + task.m_solveFriction = false; + task.run(0); + + } + } + } + + for(int iter=0; iter>2); + int remain= (wgIdx%((nSplitX*nSplitY)/4)); + int yIdx = (remain/(nSplitX/2))*2 + ((cellBatch&2)>>1); + int xIdx = (remain%(nSplitX/2))*2 + (cellBatch&1); + + int cellIdx = xIdx+yIdx*nSplitX+zIdx*(nSplitX*nSplitY); + + if( numConstraintsHost[cellIdx] == 0 ) + continue; + + //printf("yIdx=%d\n",yIdx); + + const int start = offsetsHost[cellIdx]; + int numConstraintsInCell = numConstraintsHost[cellIdx]; + // const int end = start + numConstraintsInCell; + + SolveTask task( bodyNative, shapeNative, constraintNative, start, numConstraintsInCell,maxNumBatches, 0,0,batchSizes,cellIdx); + task.m_solveFriction = true; + task.run(0); + + } + } + } + + + } else + { + for(int iter=0; itercopyFromHost(bodyNative); + shapeBuf->copyFromHost(shapeNative); + constraint->copyFromHost(constraintNative); + frame++; + +} + +void checkConstraintBatch(const b3OpenCLArray* bodyBuf, + const b3OpenCLArray* shapeBuf, + b3OpenCLArray* constraint, + b3OpenCLArray* m_numConstraints, + b3OpenCLArray* m_offsets, + int batchId + ) +{ +// b3BufferInfoCL( m_numConstraints->getBufferCL() ), +// b3BufferInfoCL( m_offsets->getBufferCL() ) + + int cellBatch = batchId; + const int nn = B3_SOLVER_N_CELLS; +// int numWorkItems = 64*nn/B3_SOLVER_N_BATCHES; + + b3AlignedObjectArray gN; + m_numConstraints->copyToHost(gN); + b3AlignedObjectArray gOffsets; + m_offsets->copyToHost(gOffsets); + int nSplitX = B3_SOLVER_N_SPLIT_X; + int nSplitY = B3_SOLVER_N_SPLIT_Y; + +// int bIdx = batchId; + + b3AlignedObjectArray cpuConstraints; + constraint->copyToHost(cpuConstraints); + + printf("batch = %d\n", batchId); + + int numWorkgroups = nn/B3_SOLVER_N_BATCHES; + b3AlignedObjectArray usedBodies; + + + for (int wgIdx=0;wgIdx>2); + int remain = wgIdx%((nSplitX*nSplitY)); + int yIdx = (remain%(nSplitX/2))*2 + ((cellBatch&2)>>1); + int xIdx = (remain/(nSplitX/2))*2 + (cellBatch&1); + + + int cellIdx = xIdx+yIdx*nSplitX+zIdx*(nSplitX*nSplitY); + printf("cellIdx=%d\n",cellIdx); + if( gN[cellIdx] == 0 ) + continue; + + const int start = gOffsets[cellIdx]; + const int end = start + gN[cellIdx]; + + for (int c=start;c* bodyBuf, const b3OpenCLArray* shapeBuf, + b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches) +{ + + + b3Int4 cdata = b3MakeInt4( n, 0, 0, 0 ); + { + + const int nn = B3_SOLVER_N_CELLS; + + cdata.x = 0; + cdata.y = maxNumBatches;//250; + + + int numWorkItems = 64*nn/B3_SOLVER_N_BATCHES; +#ifdef DEBUG_ME + SolverDebugInfo* debugInfo = new SolverDebugInfo[numWorkItems]; + adl::b3OpenCLArray gpuDebugInfo(data->m_device,numWorkItems); +#endif + + + + { + + B3_PROFILE("m_batchSolveKernel iterations"); + for(int iter=0; itergetBufferCL() ), + b3BufferInfoCL( shapeBuf->getBufferCL() ), + b3BufferInfoCL( constraint->getBufferCL() ), + b3BufferInfoCL( m_numConstraints->getBufferCL() ), + b3BufferInfoCL( m_offsets->getBufferCL() ) +#ifdef DEBUG_ME + , b3BufferInfoCL(&gpuDebugInfo) +#endif + }; + + + + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + //launcher.setConst( cdata.x ); + launcher.setConst( cdata.y ); + launcher.setConst( cdata.z ); + b3Int4 nSplit; + nSplit.x = B3_SOLVER_N_SPLIT_X; + nSplit.y = B3_SOLVER_N_SPLIT_Y; + nSplit.z = B3_SOLVER_N_SPLIT_Z; + + launcher.setConst( nSplit ); + launcher.launch1D( numWorkItems, 64 ); + + +#else + const char* fileName = "m_batchSolveKernel.bin"; + FILE* f = fopen(fileName,"rb"); + if (f) + { + int sizeInBytes=0; + if (fseek(f, 0, SEEK_END) || (sizeInBytes = ftell(f)) == EOF || fseek(f, 0, SEEK_SET)) + { + printf("error, cannot get file size\n"); + exit(0); + } + + unsigned char* buf = (unsigned char*) malloc(sizeInBytes); + fread(buf,sizeInBytes,1,f); + int serializedBytes = launcher.deserializeArgs(buf, sizeInBytes,m_context); + int num = *(int*)&buf[serializedBytes]; + + launcher.launch1D( num); + + //this clFinish is for testing on errors + clFinish(m_queue); + } + +#endif + + +#ifdef DEBUG_ME + clFinish(m_queue); + gpuDebugInfo.read(debugInfo,numWorkItems); + clFinish(m_queue); + for (int i=0;i0) + { + printf("debugInfo[i].m_valInt2 = %d\n",i,debugInfo[i].m_valInt2); + } + + if (debugInfo[i].m_valInt3>0) + { + printf("debugInfo[i].m_valInt3 = %d\n",i,debugInfo[i].m_valInt3); + } + } +#endif //DEBUG_ME + + + } + } + + clFinish(m_queue); + + + } + + cdata.x = 1; + bool applyFriction=true; + if (applyFriction) + { + B3_PROFILE("m_batchSolveKernel iterations2"); + for(int iter=0; itergetBufferCL() ), + b3BufferInfoCL( shapeBuf->getBufferCL() ), + b3BufferInfoCL( constraint->getBufferCL() ), + b3BufferInfoCL( m_numConstraints->getBufferCL() ), + b3BufferInfoCL( m_offsets->getBufferCL() ) +#ifdef DEBUG_ME + ,b3BufferInfoCL(&gpuDebugInfo) +#endif //DEBUG_ME + }; + b3LauncherCL launcher( m_queue, m_solveFrictionKernel,"m_solveFrictionKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + //launcher.setConst( cdata.x ); + launcher.setConst( cdata.y ); + launcher.setConst( cdata.z ); + b3Int4 nSplit; + nSplit.x = B3_SOLVER_N_SPLIT_X; + nSplit.y = B3_SOLVER_N_SPLIT_Y; + nSplit.z = B3_SOLVER_N_SPLIT_Z; + + launcher.setConst( nSplit ); + + launcher.launch1D( 64*nn/B3_SOLVER_N_BATCHES, 64 ); + } + } + clFinish(m_queue); + + } +#ifdef DEBUG_ME + delete[] debugInfo; +#endif //DEBUG_ME + } + + +} + +void b3Solver::convertToConstraints( const b3OpenCLArray* bodyBuf, + const b3OpenCLArray* shapeBuf, + b3OpenCLArray* contactsIn, b3OpenCLArray* contactCOut, void* additionalData, + int nContacts, const ConstraintCfg& cfg ) +{ +// b3OpenCLArray* constraintNative =0; + contactCOut->resize(nContacts); + struct CB + { + int m_nContacts; + float m_dt; + float m_positionDrift; + float m_positionConstraintCoeff; + }; + + { + + CB cdata; + cdata.m_nContacts = nContacts; + cdata.m_dt = cfg.m_dt; + cdata.m_positionDrift = cfg.m_positionDrift; + cdata.m_positionConstraintCoeff = cfg.m_positionConstraintCoeff; + + + if (gConvertConstraintOnCpu) + { + b3AlignedObjectArray gBodies; + bodyBuf->copyToHost(gBodies); + + b3AlignedObjectArray gContact; + contactsIn->copyToHost(gContact); + + b3AlignedObjectArray gShapes; + shapeBuf->copyToHost(gShapes); + + b3AlignedObjectArray gConstraintOut; + gConstraintOut.resize(nContacts); + + B3_PROFILE("cpu contactToConstraintKernel"); + for (int gIdx=0;gIdxcopyFromHost(gConstraintOut); + + } else + { + B3_PROFILE("gpu m_contactToConstraintKernel"); + + + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( bodyBuf->getBufferCL() ), b3BufferInfoCL( shapeBuf->getBufferCL()), + b3BufferInfoCL( contactCOut->getBufferCL() )}; + b3LauncherCL launcher( m_queue, m_contactToConstraintKernel,"m_contactToConstraintKernel" ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + //launcher.setConst( cdata ); + + launcher.setConst(cdata.m_nContacts); + launcher.setConst(cdata.m_dt); + launcher.setConst(cdata.m_positionDrift); + launcher.setConst(cdata.m_positionConstraintCoeff); + + launcher.launch1D( nContacts, 64 ); + clFinish(m_queue); + + } + } + + +} + +/* +void b3Solver::sortContacts( const b3OpenCLArray* bodyBuf, + b3OpenCLArray* contactsIn, void* additionalData, + int nContacts, const b3Solver::ConstraintCfg& cfg ) +{ + + + + const int sortAlignment = 512; // todo. get this out of sort + if( cfg.m_enableParallelSolve ) + { + + + int sortSize = NEXTMULTIPLEOF( nContacts, sortAlignment ); + + b3OpenCLArray* countsNative = m_numConstraints;//BufferUtils::map( data->m_device, &countsHost ); + b3OpenCLArray* offsetsNative = m_offsets;//BufferUtils::map( data->m_device, &offsetsHost ); + + { // 2. set cell idx + struct CB + { + int m_nContacts; + int m_staticIdx; + float m_scale; + int m_nSplit; + }; + + b3Assert( sortSize%64 == 0 ); + CB cdata; + cdata.m_nContacts = nContacts; + cdata.m_staticIdx = cfg.m_staticIdx; + cdata.m_scale = 1.f/(N_OBJ_PER_SPLIT*cfg.m_averageExtent); + cdata.m_nSplit = B3_SOLVER_N_SPLIT; + + + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( bodyBuf->getBufferCL() ), b3BufferInfoCL( m_sortDataBuffer->getBufferCL() ) }; + b3LauncherCL launcher( m_queue, m_setSortDataKernel ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( cdata ); + launcher.launch1D( sortSize, 64 ); + } + + { // 3. sort by cell idx + int n = B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT; + int sortBit = 32; + //if( n <= 0xffff ) sortBit = 16; + //if( n <= 0xff ) sortBit = 8; + m_sort32->execute(*m_sortDataBuffer,sortSize); + } + { // 4. find entries + m_search->execute( *m_sortDataBuffer, nContacts, *countsNative, B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT, b3BoundSearchCL::COUNT); + + m_scan->execute( *countsNative, *offsetsNative, B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT ); + } + + { // 5. sort constraints by cellIdx + // todo. preallocate this +// b3Assert( contactsIn->getType() == TYPE_HOST ); +// b3OpenCLArray* out = BufferUtils::map( data->m_device, contactsIn ); // copying contacts to this buffer + + { + + + b3Int4 cdata; cdata.x = nContacts; + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( m_contactBuffer->getBufferCL() ), b3BufferInfoCL( m_sortDataBuffer->getBufferCL() ) }; + b3LauncherCL launcher( m_queue, m_reorderContactKernel ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( cdata ); + launcher.launch1D( nContacts, 64 ); + } +// BufferUtils::unmap( out, contactsIn, nContacts ); + } + } + + +} + +*/ +void b3Solver::batchContacts( b3OpenCLArray* contacts, int nContacts, b3OpenCLArray* nNative, b3OpenCLArray* offsetsNative, int staticIdx ) +{ + + int numWorkItems = 64*B3_SOLVER_N_CELLS; + { + B3_PROFILE("batch generation"); + + b3Int4 cdata; + cdata.x = nContacts; + cdata.y = 0; + cdata.z = staticIdx; + + +#ifdef BATCH_DEBUG + SolverDebugInfo* debugInfo = new SolverDebugInfo[numWorkItems]; + adl::b3OpenCLArray gpuDebugInfo(data->m_device,numWorkItems); + memset(debugInfo,0,sizeof(SolverDebugInfo)*numWorkItems); + gpuDebugInfo.write(debugInfo,numWorkItems); +#endif + + + +#if 0 + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( contacts->getBufferCL() ), + b3BufferInfoCL( m_contactBuffer2->getBufferCL()), + b3BufferInfoCL( nNative->getBufferCL() ), + b3BufferInfoCL( offsetsNative->getBufferCL() ), +#ifdef BATCH_DEBUG + , b3BufferInfoCL(&gpuDebugInfo) +#endif + }; +#endif + + + + { + m_batchSizes.resize(nNative->size()); + B3_PROFILE("batchingKernel"); + //b3LauncherCL launcher( m_queue, m_batchingKernel); + cl_kernel k = useNewBatchingKernel ? m_batchingKernelNew : m_batchingKernel; + + b3LauncherCL launcher( m_queue, k,"*batchingKernel"); + if (!useNewBatchingKernel ) + { + launcher.setBuffer( contacts->getBufferCL() ); + } + launcher.setBuffer( m_contactBuffer2->getBufferCL() ); + launcher.setBuffer( nNative->getBufferCL()); + launcher.setBuffer( offsetsNative->getBufferCL()); + + launcher.setBuffer(m_batchSizes.getBufferCL()); + + + //launcher.setConst( cdata ); + launcher.setConst(staticIdx); + + launcher.launch1D( numWorkItems, 64 ); + //clFinish(m_queue); + //b3AlignedObjectArray batchSizesCPU; + //m_batchSizes.copyToHost(batchSizesCPU); + //printf(".\n"); + } + +#ifdef BATCH_DEBUG + aaaa + b3Contact4* hostContacts = new b3Contact4[nContacts]; + m_contactBuffer->read(hostContacts,nContacts); + clFinish(m_queue); + + gpuDebugInfo.read(debugInfo,numWorkItems); + clFinish(m_queue); + + for (int i=0;i0) + { + printf("catch\n"); + } + if (debugInfo[i].m_valInt2>0) + { + printf("catch22\n"); + } + + if (debugInfo[i].m_valInt3>0) + { + printf("catch666\n"); + } + + if (debugInfo[i].m_valInt4>0) + { + printf("catch777\n"); + } + } + delete[] debugInfo; +#endif //BATCH_DEBUG + + } + +// copy buffer to buffer + //b3Assert(m_contactBuffer->size()==nContacts); + //contacts->copyFromOpenCLArray( *m_contactBuffer); + //clFinish(m_queue);//needed? + + + +} + diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.h new file mode 100644 index 000000000000..b37f2f1bece1 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.h @@ -0,0 +1,126 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, 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. +*/ +//Originally written by Takahiro Harada + + +#ifndef __ADL_SOLVER_H +#define __ADL_SOLVER_H + +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +#include "b3GpuConstraint4.h" + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" + +#include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3BoundSearchCL.h" + +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" + + +#define B3NEXTMULTIPLEOF(num, alignment) (((num)/(alignment) + (((num)%(alignment)==0)?0:1))*(alignment)) + +enum +{ + B3_SOLVER_N_SPLIT_X = 8,//16,//4, + B3_SOLVER_N_SPLIT_Y = 4,//16,//4, + B3_SOLVER_N_SPLIT_Z = 8,//, + B3_SOLVER_N_CELLS = B3_SOLVER_N_SPLIT_X*B3_SOLVER_N_SPLIT_Y*B3_SOLVER_N_SPLIT_Z, + B3_SOLVER_N_BATCHES = 8,//4,//8,//4, + B3_MAX_NUM_BATCHES = 128, +}; + +class b3SolverBase +{ + public: + + + struct ConstraintCfg + { + ConstraintCfg( float dt = 0.f ): m_positionDrift( 0.005f ), m_positionConstraintCoeff( 0.2f ), m_dt(dt), m_staticIdx(-1) {} + + float m_positionDrift; + float m_positionConstraintCoeff; + float m_dt; + bool m_enableParallelSolve; + float m_batchCellSize; + int m_staticIdx; + }; + +}; + +class b3Solver : public b3SolverBase +{ + public: + + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; + + + b3OpenCLArray* m_numConstraints; + b3OpenCLArray* m_offsets; + b3OpenCLArray m_batchSizes; + + + int m_nIterations; + cl_kernel m_batchingKernel; + cl_kernel m_batchingKernelNew; + cl_kernel m_solveContactKernel; + cl_kernel m_solveFrictionKernel; + cl_kernel m_contactToConstraintKernel; + cl_kernel m_setSortDataKernel; + cl_kernel m_reorderContactKernel; + cl_kernel m_copyConstraintKernel; + + class b3RadixSort32CL* m_sort32; + class b3BoundSearchCL* m_search; + class b3PrefixScanCL* m_scan; + + b3OpenCLArray* m_sortDataBuffer; + b3OpenCLArray* m_contactBuffer2; + + enum + { + DYNAMIC_CONTACT_ALLOCATION_THRESHOLD = 2000000, + }; + + + + + b3Solver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity); + + virtual ~b3Solver(); + + void solveContactConstraint( const b3OpenCLArray* bodyBuf, const b3OpenCLArray* inertiaBuf, + b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches); + + void solveContactConstraintHost( b3OpenCLArray* bodyBuf, b3OpenCLArray* shapeBuf, + b3OpenCLArray* constraint, void* additionalData, int n ,int maxNumBatches, b3AlignedObjectArray* batchSizes); + + + void convertToConstraints( const b3OpenCLArray* bodyBuf, + const b3OpenCLArray* shapeBuf, + b3OpenCLArray* contactsIn, b3OpenCLArray* contactCOut, void* additionalData, + int nContacts, const ConstraintCfg& cfg ); + + void batchContacts( b3OpenCLArray* contacts, int nContacts, b3OpenCLArray* n, b3OpenCLArray* offsets, int staticIdx ); + +}; + + + + +#endif //__ADL_SOLVER_H diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl new file mode 100644 index 000000000000..3b891b863d4d --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl @@ -0,0 +1,353 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, 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. +*/ +//Originally written by Takahiro Harada + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable + +#ifdef cl_ext_atomic_counters_32 +#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable +#else +#define counter32_t volatile __global int* +#endif + + +typedef unsigned int u32; +typedef unsigned short u16; +typedef unsigned char u8; + +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GET_NUM_GROUPS get_num_groups(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) +#define AppendInc(x, out) out = atomic_inc(x) +#define AtomAdd(x, value) atom_add(&(x), value) +#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value ) +#define AtomXhg(x, value) atom_xchg ( &(x), value ) + + +#define SELECT_UINT4( b, a, condition ) select( b,a,condition ) + +#define make_float4 (float4) +#define make_float2 (float2) +#define make_uint4 (uint4) +#define make_int4 (int4) +#define make_uint2 (uint2) +#define make_int2 (int2) + + +#define max2 max +#define min2 min + + +#define WG_SIZE 64 + + + + + +typedef struct +{ + int m_n; + int m_start; + int m_staticIdx; + int m_paddings[1]; +} ConstBuffer; + +typedef struct +{ + int m_a; + int m_b; + u32 m_idx; +}Elem; + +#define STACK_SIZE (WG_SIZE*10) +//#define STACK_SIZE (WG_SIZE) +#define RING_SIZE 1024 +#define RING_SIZE_MASK (RING_SIZE-1) +#define CHECK_SIZE (WG_SIZE) + + +#define GET_RING_CAPACITY (RING_SIZE - ldsRingEnd) +#define RING_END ldsTmp + +u32 readBuf(__local u32* buff, int idx) +{ + idx = idx % (32*CHECK_SIZE); + int bitIdx = idx%32; + int bufIdx = idx/32; + return buff[bufIdx] & (1<> bitIdx)&1) == 0; +} + +// batching on the GPU +__kernel void CreateBatches( __global const struct b3Contact4Data* gConstraints, __global struct b3Contact4Data* gConstraintsOut, + __global const u32* gN, __global const u32* gStart, __global int* batchSizes, + int m_staticIdx ) +{ + __local u32 ldsStackIdx[STACK_SIZE]; + __local u32 ldsStackEnd; + __local Elem ldsRingElem[RING_SIZE]; + __local u32 ldsRingEnd; + __local u32 ldsTmp; + __local u32 ldsCheckBuffer[CHECK_SIZE]; + __local u32 ldsFixedBuffer[CHECK_SIZE]; + __local u32 ldsGEnd; + __local u32 ldsDstEnd; + + int wgIdx = GET_GROUP_IDX; + int lIdx = GET_LOCAL_IDX; + + const int m_n = gN[wgIdx]; + const int m_start = gStart[wgIdx]; + + if( lIdx == 0 ) + { + ldsRingEnd = 0; + ldsGEnd = 0; + ldsStackEnd = 0; + ldsDstEnd = m_start; + } + + + +// while(1) +//was 250 + int ie=0; + int maxBatch = 0; + for(ie=0; ie<50; ie++) + { + ldsFixedBuffer[lIdx] = 0; + + for(int giter=0; giter<4; giter++) + { + int ringCap = GET_RING_CAPACITY; + + // 1. fill ring + if( ldsGEnd < m_n ) + { + while( ringCap > WG_SIZE ) + { + if( ldsGEnd >= m_n ) break; + if( lIdx < ringCap - WG_SIZE ) + { + int srcIdx; + AtomInc1( ldsGEnd, srcIdx ); + if( srcIdx < m_n ) + { + int dstIdx; + AtomInc1( ldsRingEnd, dstIdx ); + + int a = gConstraints[m_start+srcIdx].m_bodyAPtrAndSignBit; + int b = gConstraints[m_start+srcIdx].m_bodyBPtrAndSignBit; + ldsRingElem[dstIdx].m_a = (a>b)? b:a; + ldsRingElem[dstIdx].m_b = (a>b)? a:b; + ldsRingElem[dstIdx].m_idx = srcIdx; + } + } + ringCap = GET_RING_CAPACITY; + } + } + + GROUP_LDS_BARRIER; + + // 2. fill stack + __local Elem* dst = ldsRingElem; + if( lIdx == 0 ) RING_END = 0; + + int srcIdx=lIdx; + int end = ldsRingEnd; + + { + for(int ii=0; ii1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" +" return false;\n" +" return true;\n" +"}\n" +"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" +"{\n" +" float maxDot = -B3_INFINITY;\n" +" int i = 0;\n" +" int ptIndex = -1;\n" +" for( i = 0; i < vecLen; i++ )\n" +" {\n" +" float dot = b3Dot3F4(vecArray[i],vec);\n" +" \n" +" if( dot > maxDot )\n" +" {\n" +" maxDot = dot;\n" +" ptIndex = i;\n" +" }\n" +" }\n" +" b3Assert(ptIndex>=0);\n" +" if (ptIndex<0)\n" +" {\n" +" ptIndex = 0;\n" +" }\n" +" *dotOut = maxDot;\n" +" return ptIndex;\n" +"}\n" +"#endif //B3_FLOAT4_H\n" +"typedef struct b3Contact4Data b3Contact4Data_t;\n" +"struct b3Contact4Data\n" +"{\n" +" b3Float4 m_worldPosB[4];\n" +"// b3Float4 m_localPosA[4];\n" +"// b3Float4 m_localPosB[4];\n" +" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" +" unsigned short m_restituitionCoeffCmp;\n" +" unsigned short m_frictionCoeffCmp;\n" +" int m_batchIdx;\n" +" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" +" int m_bodyBPtrAndSignBit;\n" +" int m_childIndexA;\n" +" int m_childIndexB;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"};\n" +"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" +"{\n" +" return (int)contact->m_worldNormalOnB.w;\n" +"};\n" +"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" +"{\n" +" contact->m_worldNormalOnB.w = (float)numPoints;\n" +"};\n" +"#endif //B3_CONTACT4DATA_H\n" +"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" +"#ifdef cl_ext_atomic_counters_32\n" +"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" +"#else\n" +"#define counter32_t volatile __global int*\n" +"#endif\n" +"typedef unsigned int u32;\n" +"typedef unsigned short u16;\n" +"typedef unsigned char u8;\n" +"#define GET_GROUP_IDX get_group_id(0)\n" +"#define GET_LOCAL_IDX get_local_id(0)\n" +"#define GET_GLOBAL_IDX get_global_id(0)\n" +"#define GET_GROUP_SIZE get_local_size(0)\n" +"#define GET_NUM_GROUPS get_num_groups(0)\n" +"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" +"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" +"#define AtomInc(x) atom_inc(&(x))\n" +"#define AtomInc1(x, out) out = atom_inc(&(x))\n" +"#define AppendInc(x, out) out = atomic_inc(x)\n" +"#define AtomAdd(x, value) atom_add(&(x), value)\n" +"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" +"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" +"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" +"#define make_float4 (float4)\n" +"#define make_float2 (float2)\n" +"#define make_uint4 (uint4)\n" +"#define make_int4 (int4)\n" +"#define make_uint2 (uint2)\n" +"#define make_int2 (int2)\n" +"#define max2 max\n" +"#define min2 min\n" +"#define WG_SIZE 64\n" +"typedef struct \n" +"{\n" +" int m_n;\n" +" int m_start;\n" +" int m_staticIdx;\n" +" int m_paddings[1];\n" +"} ConstBuffer;\n" +"typedef struct \n" +"{\n" +" int m_a;\n" +" int m_b;\n" +" u32 m_idx;\n" +"}Elem;\n" +"#define STACK_SIZE (WG_SIZE*10)\n" +"//#define STACK_SIZE (WG_SIZE)\n" +"#define RING_SIZE 1024\n" +"#define RING_SIZE_MASK (RING_SIZE-1)\n" +"#define CHECK_SIZE (WG_SIZE)\n" +"#define GET_RING_CAPACITY (RING_SIZE - ldsRingEnd)\n" +"#define RING_END ldsTmp\n" +"u32 readBuf(__local u32* buff, int idx)\n" +"{\n" +" idx = idx % (32*CHECK_SIZE);\n" +" int bitIdx = idx%32;\n" +" int bufIdx = idx/32;\n" +" return buff[bufIdx] & (1<> bitIdx)&1) == 0;\n" +"}\n" +"// batching on the GPU\n" +"__kernel void CreateBatches( __global const struct b3Contact4Data* gConstraints, __global struct b3Contact4Data* gConstraintsOut,\n" +" __global const u32* gN, __global const u32* gStart, __global int* batchSizes, \n" +" int m_staticIdx )\n" +"{\n" +" __local u32 ldsStackIdx[STACK_SIZE];\n" +" __local u32 ldsStackEnd;\n" +" __local Elem ldsRingElem[RING_SIZE];\n" +" __local u32 ldsRingEnd;\n" +" __local u32 ldsTmp;\n" +" __local u32 ldsCheckBuffer[CHECK_SIZE];\n" +" __local u32 ldsFixedBuffer[CHECK_SIZE];\n" +" __local u32 ldsGEnd;\n" +" __local u32 ldsDstEnd;\n" +" int wgIdx = GET_GROUP_IDX;\n" +" int lIdx = GET_LOCAL_IDX;\n" +" \n" +" const int m_n = gN[wgIdx];\n" +" const int m_start = gStart[wgIdx];\n" +" \n" +" if( lIdx == 0 )\n" +" {\n" +" ldsRingEnd = 0;\n" +" ldsGEnd = 0;\n" +" ldsStackEnd = 0;\n" +" ldsDstEnd = m_start;\n" +" }\n" +" \n" +" \n" +" \n" +"// while(1)\n" +"//was 250\n" +" int ie=0;\n" +" int maxBatch = 0;\n" +" for(ie=0; ie<50; ie++)\n" +" {\n" +" ldsFixedBuffer[lIdx] = 0;\n" +" for(int giter=0; giter<4; giter++)\n" +" {\n" +" int ringCap = GET_RING_CAPACITY;\n" +" \n" +" // 1. fill ring\n" +" if( ldsGEnd < m_n )\n" +" {\n" +" while( ringCap > WG_SIZE )\n" +" {\n" +" if( ldsGEnd >= m_n ) break;\n" +" if( lIdx < ringCap - WG_SIZE )\n" +" {\n" +" int srcIdx;\n" +" AtomInc1( ldsGEnd, srcIdx );\n" +" if( srcIdx < m_n )\n" +" {\n" +" int dstIdx;\n" +" AtomInc1( ldsRingEnd, dstIdx );\n" +" \n" +" int a = gConstraints[m_start+srcIdx].m_bodyAPtrAndSignBit;\n" +" int b = gConstraints[m_start+srcIdx].m_bodyBPtrAndSignBit;\n" +" ldsRingElem[dstIdx].m_a = (a>b)? b:a;\n" +" ldsRingElem[dstIdx].m_b = (a>b)? a:b;\n" +" ldsRingElem[dstIdx].m_idx = srcIdx;\n" +" }\n" +" }\n" +" ringCap = GET_RING_CAPACITY;\n" +" }\n" +" }\n" +" GROUP_LDS_BARRIER;\n" +" \n" +" // 2. fill stack\n" +" __local Elem* dst = ldsRingElem;\n" +" if( lIdx == 0 ) RING_END = 0;\n" +" int srcIdx=lIdx;\n" +" int end = ldsRingEnd;\n" +" {\n" +" for(int ii=0; ii> bitIdx)&1) == 0; +} + + +// batching on the GPU +__kernel void CreateBatchesNew( __global struct b3Contact4Data* gConstraints, __global const u32* gN, __global const u32* gStart, __global int* batchSizes, int staticIdx ) +{ + int wgIdx = GET_GROUP_IDX; + int lIdx = GET_LOCAL_IDX; + const int numConstraints = gN[wgIdx]; + const int m_start = gStart[wgIdx]; + b3Contact4Data_t tmp; + + __local u32 ldsFixedBuffer[CHECK_SIZE]; + + + + + + if( lIdx == 0 ) + { + + + __global struct b3Contact4Data* cs = &gConstraints[m_start]; + + + int numValidConstraints = 0; + int batchIdx = 0; + + while( numValidConstraints < numConstraints) + { + int nCurrentBatch = 0; + // clear flag + + for(int i=0; i1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" +" return false;\n" +" return true;\n" +"}\n" +"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" +"{\n" +" float maxDot = -B3_INFINITY;\n" +" int i = 0;\n" +" int ptIndex = -1;\n" +" for( i = 0; i < vecLen; i++ )\n" +" {\n" +" float dot = b3Dot3F4(vecArray[i],vec);\n" +" \n" +" if( dot > maxDot )\n" +" {\n" +" maxDot = dot;\n" +" ptIndex = i;\n" +" }\n" +" }\n" +" b3Assert(ptIndex>=0);\n" +" if (ptIndex<0)\n" +" {\n" +" ptIndex = 0;\n" +" }\n" +" *dotOut = maxDot;\n" +" return ptIndex;\n" +"}\n" +"#endif //B3_FLOAT4_H\n" +"typedef struct b3Contact4Data b3Contact4Data_t;\n" +"struct b3Contact4Data\n" +"{\n" +" b3Float4 m_worldPosB[4];\n" +"// b3Float4 m_localPosA[4];\n" +"// b3Float4 m_localPosB[4];\n" +" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" +" unsigned short m_restituitionCoeffCmp;\n" +" unsigned short m_frictionCoeffCmp;\n" +" int m_batchIdx;\n" +" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" +" int m_bodyBPtrAndSignBit;\n" +" int m_childIndexA;\n" +" int m_childIndexB;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"};\n" +"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" +"{\n" +" return (int)contact->m_worldNormalOnB.w;\n" +"};\n" +"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" +"{\n" +" contact->m_worldNormalOnB.w = (float)numPoints;\n" +"};\n" +"#endif //B3_CONTACT4DATA_H\n" +"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" +"#ifdef cl_ext_atomic_counters_32\n" +"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" +"#else\n" +"#define counter32_t volatile __global int*\n" +"#endif\n" +"#define SIMD_WIDTH 64\n" +"typedef unsigned int u32;\n" +"typedef unsigned short u16;\n" +"typedef unsigned char u8;\n" +"#define GET_GROUP_IDX get_group_id(0)\n" +"#define GET_LOCAL_IDX get_local_id(0)\n" +"#define GET_GLOBAL_IDX get_global_id(0)\n" +"#define GET_GROUP_SIZE get_local_size(0)\n" +"#define GET_NUM_GROUPS get_num_groups(0)\n" +"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" +"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" +"#define AtomInc(x) atom_inc(&(x))\n" +"#define AtomInc1(x, out) out = atom_inc(&(x))\n" +"#define AppendInc(x, out) out = atomic_inc(x)\n" +"#define AtomAdd(x, value) atom_add(&(x), value)\n" +"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" +"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" +"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" +"#define make_float4 (float4)\n" +"#define make_float2 (float2)\n" +"#define make_uint4 (uint4)\n" +"#define make_int4 (int4)\n" +"#define make_uint2 (uint2)\n" +"#define make_int2 (int2)\n" +"#define max2 max\n" +"#define min2 min\n" +"#define WG_SIZE 64\n" +"typedef struct \n" +"{\n" +" int m_n;\n" +" int m_start;\n" +" int m_staticIdx;\n" +" int m_paddings[1];\n" +"} ConstBuffer;\n" +"typedef struct \n" +"{\n" +" int m_a;\n" +" int m_b;\n" +" u32 m_idx;\n" +"}Elem;\n" +"// batching on the GPU\n" +"__kernel void CreateBatchesBruteForce( __global struct b3Contact4Data* gConstraints, __global const u32* gN, __global const u32* gStart, int m_staticIdx )\n" +"{\n" +" int wgIdx = GET_GROUP_IDX;\n" +" int lIdx = GET_LOCAL_IDX;\n" +" \n" +" const int m_n = gN[wgIdx];\n" +" const int m_start = gStart[wgIdx];\n" +" \n" +" if( lIdx == 0 )\n" +" {\n" +" for (int i=0;i> bitIdx)&1) == 0;\n" +"}\n" +"// batching on the GPU\n" +"__kernel void CreateBatchesNew( __global struct b3Contact4Data* gConstraints, __global const u32* gN, __global const u32* gStart, __global int* batchSizes, int staticIdx )\n" +"{\n" +" int wgIdx = GET_GROUP_IDX;\n" +" int lIdx = GET_LOCAL_IDX;\n" +" const int numConstraints = gN[wgIdx];\n" +" const int m_start = gStart[wgIdx];\n" +" b3Contact4Data_t tmp;\n" +" \n" +" __local u32 ldsFixedBuffer[CHECK_SIZE];\n" +" \n" +" \n" +" \n" +" \n" +" \n" +" if( lIdx == 0 )\n" +" {\n" +" \n" +" \n" +" __global struct b3Contact4Data* cs = &gConstraints[m_start]; \n" +" \n" +" \n" +" int numValidConstraints = 0;\n" +" int batchIdx = 0;\n" +" while( numValidConstraints < numConstraints)\n" +" {\n" +" int nCurrentBatch = 0;\n" +" // clear flag\n" +" \n" +" for(int i=0; i1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" +" return false;\n" +" return true;\n" +"}\n" +"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" +"{\n" +" float maxDot = -B3_INFINITY;\n" +" int i = 0;\n" +" int ptIndex = -1;\n" +" for( i = 0; i < vecLen; i++ )\n" +" {\n" +" float dot = b3Dot3F4(vecArray[i],vec);\n" +" \n" +" if( dot > maxDot )\n" +" {\n" +" maxDot = dot;\n" +" ptIndex = i;\n" +" }\n" +" }\n" +" b3Assert(ptIndex>=0);\n" +" if (ptIndex<0)\n" +" {\n" +" ptIndex = 0;\n" +" }\n" +" *dotOut = maxDot;\n" +" return ptIndex;\n" +"}\n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_QUAT_H\n" +"#define B3_QUAT_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif\n" +"#endif\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Quat;\n" +" #define b3QuatConstArg const b3Quat\n" +" \n" +" \n" +"inline float4 b3FastNormalize4(float4 v)\n" +"{\n" +" v = (float4)(v.xyz,0.f);\n" +" return fast_normalize(v);\n" +"}\n" +" \n" +"inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" +"inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" +"inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" +"{\n" +" b3Quat ans;\n" +" ans = b3Cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - b3Dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" +"{\n" +" b3Quat q;\n" +" q=in;\n" +" //return b3FastNormalize4(in);\n" +" float len = native_sqrt(dot(q, q));\n" +" if(len > 0.f)\n" +" {\n" +" q *= 1.f / len;\n" +" }\n" +" else\n" +" {\n" +" q.x = q.y = q.z = 0.f;\n" +" q.w = 1.f;\n" +" }\n" +" return q;\n" +"}\n" +"inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" b3Quat qInv = b3QuatInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" return b3QuatRotate( b3QuatInvert( q ), vec );\n" +"}\n" +"inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" +"{\n" +" return b3QuatRotate( orientation, point ) + (translation);\n" +"}\n" +" \n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifndef B3_MAT3x3_H\n" +"#define B3_MAT3x3_H\n" +"#ifndef B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"typedef struct\n" +"{\n" +" b3Float4 m_row[3];\n" +"}b3Mat3x3;\n" +"#define b3Mat3x3ConstArg const b3Mat3x3\n" +"#define b3GetRow(m,row) (m.m_row[row])\n" +"inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" +"{\n" +" b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" +" b3Mat3x3 out;\n" +" out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" +" out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" +" out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" +" out.m_row[0].w = 0.f;\n" +" out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" +" out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" +" out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" +" out.m_row[1].w = 0.f;\n" +" out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" +" out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" +" out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" +" out.m_row[2].w = 0.f;\n" +" return out;\n" +"}\n" +"inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = fabs(matIn.m_row[0]);\n" +" out.m_row[1] = fabs(matIn.m_row[1]);\n" +" out.m_row[2] = fabs(matIn.m_row[2]);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtZero();\n" +"__inline\n" +"b3Mat3x3 mtIdentity();\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Mat3x3 mtZero()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(0.f);\n" +" m.m_row[1] = (b3Float4)(0.f);\n" +" m.m_row[2] = (b3Float4)(0.f);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtIdentity()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(1,0,0,0);\n" +" m.m_row[1] = (b3Float4)(0,1,0,0);\n" +" m.m_row[2] = (b3Float4)(0,0,1,0);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" +" out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" +" out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" +"{\n" +" b3Mat3x3 transB;\n" +" transB = mtTranspose( b );\n" +" b3Mat3x3 ans;\n" +" // why this doesn't run when 0ing in the for{}\n" +" a.m_row[0].w = 0.f;\n" +" a.m_row[1].w = 0.f;\n" +" a.m_row[2].w = 0.f;\n" +" for(int i=0; i<3; i++)\n" +" {\n" +"// a.m_row[i].w = 0.f;\n" +" ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" +" ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" +" ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" +" ans.m_row[i].w = 0.f;\n" +" }\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" +"{\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a.m_row[0], b );\n" +" ans.y = b3Dot3F4( a.m_row[1], b );\n" +" ans.z = b3Dot3F4( a.m_row[2], b );\n" +" ans.w = 0.f;\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" +"{\n" +" b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" +" b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" +" b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a, colx );\n" +" ans.y = b3Dot3F4( a, coly );\n" +" ans.z = b3Dot3F4( a, colz );\n" +" return ans;\n" +"}\n" +"#endif\n" +"#endif //B3_MAT3x3_H\n" +"typedef struct b3RigidBodyData b3RigidBodyData_t;\n" +"struct b3RigidBodyData\n" +"{\n" +" b3Float4 m_pos;\n" +" b3Quat m_quat;\n" +" b3Float4 m_linVel;\n" +" b3Float4 m_angVel;\n" +" int m_collidableIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"};\n" +"typedef struct b3InertiaData b3InertiaData_t;\n" +"struct b3InertiaData\n" +"{\n" +" b3Mat3x3 m_invInertiaWorld;\n" +" b3Mat3x3 m_initInvInertia;\n" +"};\n" +"#endif //B3_RIGIDBODY_DATA_H\n" +" \n" +"#ifndef B3_RIGIDBODY_DATA_H\n" +"#endif //B3_RIGIDBODY_DATA_H\n" +" \n" +"inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)\n" +"{\n" +" \n" +" if (bodies[nodeID].m_invMass != 0.f)\n" +" {\n" +" float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);\n" +" //angular velocity\n" +" {\n" +" b3Float4 axis;\n" +" //add some hardcoded angular damping\n" +" bodies[nodeID].m_angVel.x *= angularDamping;\n" +" bodies[nodeID].m_angVel.y *= angularDamping;\n" +" bodies[nodeID].m_angVel.z *= angularDamping;\n" +" \n" +" b3Float4 angvel = bodies[nodeID].m_angVel;\n" +" float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));\n" +" \n" +" //limit the angular motion\n" +" if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)\n" +" {\n" +" fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;\n" +" }\n" +" if(fAngle < 0.001f)\n" +" {\n" +" // use Taylor's expansions of sync function\n" +" axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);\n" +" }\n" +" else\n" +" {\n" +" // sync(fAngle) = sin(c*fAngle)/t\n" +" axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);\n" +" }\n" +" \n" +" b3Quat dorn;\n" +" dorn.x = axis.x;\n" +" dorn.y = axis.y;\n" +" dorn.z = axis.z;\n" +" dorn.w = b3Cos(fAngle * timeStep * 0.5f);\n" +" b3Quat orn0 = bodies[nodeID].m_quat;\n" +" b3Quat predictedOrn = b3QuatMul(dorn, orn0);\n" +" predictedOrn = b3QuatNormalized(predictedOrn);\n" +" bodies[nodeID].m_quat=predictedOrn;\n" +" }\n" +" //linear velocity \n" +" bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;\n" +" \n" +" //apply gravity\n" +" bodies[nodeID].m_linVel += gravityAcceleration * timeStep;\n" +" \n" +" }\n" +" \n" +"}\n" +"inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)\n" +"{\n" +" float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);\n" +" \n" +" if( (body->m_invMass != 0.f))\n" +" {\n" +" //angular velocity\n" +" {\n" +" b3Float4 axis;\n" +" //add some hardcoded angular damping\n" +" body->m_angVel.x *= angularDamping;\n" +" body->m_angVel.y *= angularDamping;\n" +" body->m_angVel.z *= angularDamping;\n" +" \n" +" b3Float4 angvel = body->m_angVel;\n" +" float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));\n" +" //limit the angular motion\n" +" if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)\n" +" {\n" +" fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;\n" +" }\n" +" if(fAngle < 0.001f)\n" +" {\n" +" // use Taylor's expansions of sync function\n" +" axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);\n" +" }\n" +" else\n" +" {\n" +" // sync(fAngle) = sin(c*fAngle)/t\n" +" axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);\n" +" }\n" +" b3Quat dorn;\n" +" dorn.x = axis.x;\n" +" dorn.y = axis.y;\n" +" dorn.z = axis.z;\n" +" dorn.w = b3Cos(fAngle * timeStep * 0.5f);\n" +" b3Quat orn0 = body->m_quat;\n" +" b3Quat predictedOrn = b3QuatMul(dorn, orn0);\n" +" predictedOrn = b3QuatNormalized(predictedOrn);\n" +" body->m_quat=predictedOrn;\n" +" }\n" +" //apply gravity\n" +" body->m_linVel += gravityAcceleration * timeStep;\n" +" //linear velocity \n" +" body->m_pos += body->m_linVel * timeStep;\n" +" \n" +" }\n" +" \n" +"}\n" +"__kernel void \n" +" integrateTransformsKernel( __global b3RigidBodyData_t* bodies,const int numNodes, float timeStep, float angularDamping, float4 gravityAcceleration)\n" +"{\n" +" int nodeID = get_global_id(0);\n" +" \n" +" if( nodeID < numNodes)\n" +" {\n" +" integrateSingleTransform(bodies,nodeID, timeStep, angularDamping,gravityAcceleration);\n" +" }\n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl new file mode 100644 index 000000000000..7f5dabe274d5 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl @@ -0,0 +1,877 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + +#define B3_CONSTRAINT_FLAG_ENABLED 1 + +#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3 +#define B3_GPU_FIXED_CONSTRAINT_TYPE 4 + +#define MOTIONCLAMP 100000 //unused, for debugging/safety in case constraint solver fails +#define B3_INFINITY 1e30f + +#define mymake_float4 (float4) + + +__inline float dot3F4(float4 a, float4 b) +{ + float4 a1 = mymake_float4(a.xyz,0.f); + float4 b1 = mymake_float4(b.xyz,0.f); + return dot(a1, b1); +} + + +typedef float4 Quaternion; + + +typedef struct +{ + float4 m_row[3]; +}Matrix3x3; + +__inline +float4 mtMul1(Matrix3x3 a, float4 b); + +__inline +float4 mtMul3(float4 a, Matrix3x3 b); + + + + + +__inline +float4 mtMul1(Matrix3x3 a, float4 b) +{ + float4 ans; + ans.x = dot3F4( a.m_row[0], b ); + ans.y = dot3F4( a.m_row[1], b ); + ans.z = dot3F4( a.m_row[2], b ); + ans.w = 0.f; + return ans; +} + +__inline +float4 mtMul3(float4 a, Matrix3x3 b) +{ + float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0); + float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0); + float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0); + + float4 ans; + ans.x = dot3F4( a, colx ); + ans.y = dot3F4( a, coly ); + ans.z = dot3F4( a, colz ); + return ans; +} + + + +typedef struct +{ + Matrix3x3 m_invInertiaWorld; + Matrix3x3 m_initInvInertia; +} BodyInertia; + + +typedef struct +{ + Matrix3x3 m_basis;//orientation + float4 m_origin;//transform +}b3Transform; + +typedef struct +{ +// b3Transform m_worldTransformUnused; + float4 m_deltaLinearVelocity; + float4 m_deltaAngularVelocity; + float4 m_angularFactor; + float4 m_linearFactor; + float4 m_invMass; + float4 m_pushVelocity; + float4 m_turnVelocity; + float4 m_linearVelocity; + float4 m_angularVelocity; + + union + { + void* m_originalBody; + int m_originalBodyIndex; + }; + int padding[3]; + +} b3GpuSolverBody; + +typedef struct +{ + float4 m_pos; + Quaternion m_quat; + float4 m_linVel; + float4 m_angVel; + + unsigned int m_shapeIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} b3RigidBodyCL; + +typedef struct +{ + + float4 m_relpos1CrossNormal; + float4 m_contactNormal; + + float4 m_relpos2CrossNormal; + //float4 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal + + float4 m_angularComponentA; + float4 m_angularComponentB; + + float m_appliedPushImpulse; + float m_appliedImpulse; + int m_padding1; + int m_padding2; + float m_friction; + float m_jacDiagABInv; + float m_rhs; + float m_cfm; + + float m_lowerLimit; + float m_upperLimit; + float m_rhsPenetration; + int m_originalConstraint; + + + int m_overrideNumSolverIterations; + int m_frictionIndex; + int m_solverBodyIdA; + int m_solverBodyIdB; + +} b3SolverConstraint; + +typedef struct +{ + int m_bodyAPtrAndSignBit; + int m_bodyBPtrAndSignBit; + int m_originalConstraintIndex; + int m_batchId; +} b3BatchConstraint; + + + + + + +typedef struct +{ + int m_constraintType; + int m_rbA; + int m_rbB; + float m_breakingImpulseThreshold; + + float4 m_pivotInA; + float4 m_pivotInB; + Quaternion m_relTargetAB; + + int m_flags; + int m_padding[3]; +} b3GpuGenericConstraint; + + +/*b3Transform getWorldTransform(b3RigidBodyCL* rb) +{ + b3Transform newTrans; + newTrans.setOrigin(rb->m_pos); + newTrans.setRotation(rb->m_quat); + return newTrans; +}*/ + + + + +__inline +float4 cross3(float4 a, float4 b) +{ + return cross(a,b); +} + +__inline +float4 fastNormalize4(float4 v) +{ + v = mymake_float4(v.xyz,0.f); + return fast_normalize(v); +} + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b); + +__inline +Quaternion qtNormalize(Quaternion in); + +__inline +float4 qtRotate(Quaternion q, float4 vec); + +__inline +Quaternion qtInvert(Quaternion q); + + + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b) +{ + Quaternion ans; + ans = cross3( a, b ); + ans += a.w*b+b.w*a; +// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - dot3F4(a, b); + return ans; +} + +__inline +Quaternion qtNormalize(Quaternion in) +{ + return fastNormalize4(in); +// in /= length( in ); +// return in; +} +__inline +float4 qtRotate(Quaternion q, float4 vec) +{ + Quaternion qInv = qtInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = qtMul(qtMul(q,vcpy),qInv); + return out; +} + +__inline +Quaternion qtInvert(Quaternion q) +{ + return (Quaternion)(-q.xyz, q.w); +} + + +__inline void internalApplyImpulse(__global b3GpuSolverBody* body, float4 linearComponent, float4 angularComponent,float impulseMagnitude) +{ + body->m_deltaLinearVelocity += linearComponent*impulseMagnitude*body->m_linearFactor; + body->m_deltaAngularVelocity += angularComponent*(impulseMagnitude*body->m_angularFactor); +} + + +void resolveSingleConstraintRowGeneric(__global b3GpuSolverBody* body1, __global b3GpuSolverBody* body2, __global b3SolverConstraint* c) +{ + float deltaImpulse = c->m_rhs-c->m_appliedImpulse*c->m_cfm; + float deltaVel1Dotn = dot3F4(c->m_contactNormal,body1->m_deltaLinearVelocity) + dot3F4(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity); + float deltaVel2Dotn = -dot3F4(c->m_contactNormal,body2->m_deltaLinearVelocity) + dot3F4(c->m_relpos2CrossNormal,body2->m_deltaAngularVelocity); + + deltaImpulse -= deltaVel1Dotn*c->m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c->m_jacDiagABInv; + + float sum = c->m_appliedImpulse + deltaImpulse; + if (sum < c->m_lowerLimit) + { + deltaImpulse = c->m_lowerLimit-c->m_appliedImpulse; + c->m_appliedImpulse = c->m_lowerLimit; + } + else if (sum > c->m_upperLimit) + { + deltaImpulse = c->m_upperLimit-c->m_appliedImpulse; + c->m_appliedImpulse = c->m_upperLimit; + } + else + { + c->m_appliedImpulse = sum; + } + + internalApplyImpulse(body1,c->m_contactNormal*body1->m_invMass,c->m_angularComponentA,deltaImpulse); + internalApplyImpulse(body2,-c->m_contactNormal*body2->m_invMass,c->m_angularComponentB,deltaImpulse); + +} + +__kernel void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies, + __global b3BatchConstraint* batchConstraints, + __global b3SolverConstraint* rows, + __global unsigned int* numConstraintRowsInfo1, + __global unsigned int* rowOffsets, + __global b3GpuGenericConstraint* constraints, + int batchOffset, + int numConstraintsInBatch + ) +{ + int b = get_global_id(0); + if (b>=numConstraintsInBatch) + return; + + __global b3BatchConstraint* c = &batchConstraints[b+batchOffset]; + int originalConstraintIndex = c->m_originalConstraintIndex; + if (constraints[originalConstraintIndex].m_flags&B3_CONSTRAINT_FLAG_ENABLED) + { + int numConstraintRows = numConstraintRowsInfo1[originalConstraintIndex]; + int rowOffset = rowOffsets[originalConstraintIndex]; + for (int jj=0;jjm_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint); + } + } +}; + +__kernel void initSolverBodies(__global b3GpuSolverBody* solverBodies,__global b3RigidBodyCL* bodiesCL, int numBodies) +{ + int i = get_global_id(0); + if (i>=numBodies) + return; + + __global b3GpuSolverBody* solverBody = &solverBodies[i]; + __global b3RigidBodyCL* bodyCL = &bodiesCL[i]; + + solverBody->m_deltaLinearVelocity = (float4)(0.f,0.f,0.f,0.f); + solverBody->m_deltaAngularVelocity = (float4)(0.f,0.f,0.f,0.f); + solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f); + solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f); + solverBody->m_invMass = (float4)(bodyCL->m_invMass,bodyCL->m_invMass,bodyCL->m_invMass,0.f); + solverBody->m_originalBodyIndex = i; + solverBody->m_angularFactor = (float4)(1,1,1,0); + solverBody->m_linearFactor = (float4) (1,1,1,0); + solverBody->m_linearVelocity = bodyCL->m_linVel; + solverBody->m_angularVelocity = bodyCL->m_angVel; +} + +__kernel void breakViolatedConstraintsKernel(__global b3GpuGenericConstraint* constraints, __global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, __global b3SolverConstraint* rows, int numConstraints) +{ + int cid = get_global_id(0); + if (cid>=numConstraints) + return; + int numRows = numConstraintRows[cid]; + if (numRows) + { + for (int i=0;i= breakingThreshold) + { + constraints[cid].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED; + } + } + } +} + + + +__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, int numConstraints) +{ + int i = get_global_id(0); + if (i>=numConstraints) + return; + + __global b3GpuGenericConstraint* constraint = &constraints[i]; + + switch (constraint->m_constraintType) + { + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + { + infos[i] = 3; + break; + } + case B3_GPU_FIXED_CONSTRAINT_TYPE: + { + infos[i] = 6; + break; + } + default: + { + } + } +} + +__kernel void initBatchConstraintsKernel(__global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, + __global b3BatchConstraint* batchConstraints, + __global b3GpuGenericConstraint* constraints, + __global b3RigidBodyCL* bodies, + int numConstraints) +{ + int i = get_global_id(0); + if (i>=numConstraints) + return; + + int rbA = constraints[i].m_rbA; + int rbB = constraints[i].m_rbB; + + batchConstraints[i].m_bodyAPtrAndSignBit = bodies[rbA].m_invMass != 0.f ? rbA : -rbA; + batchConstraints[i].m_bodyBPtrAndSignBit = bodies[rbB].m_invMass != 0.f ? rbB : -rbB; + batchConstraints[i].m_batchId = -1; + batchConstraints[i].m_originalConstraintIndex = i; + +} + + + + +typedef struct +{ + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + float fps,erp; + + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + union + { + __global float4* m_J1linearAxisFloat4; + __global float* m_J1linearAxis; + }; + union + { + __global float4* m_J1angularAxisFloat4; + __global float* m_J1angularAxis; + + }; + union + { + __global float4* m_J2linearAxisFloat4; + __global float* m_J2linearAxis; + }; + union + { + __global float4* m_J2angularAxisFloat4; + __global float* m_J2angularAxis; + }; + // elements to jump from one row to the next in J's + int rowskip; + + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + __global float* m_constraintError; + __global float* cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + __global float* m_lowerLimit; + __global float* m_upperLimit; + + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + __global int *findex; + // number of solver iterations + int m_numIterations; + + //damping of the velocity + float m_damping; +} b3GpuConstraintInfo2; + + +void getSkewSymmetricMatrix(float4 vecIn, __global float4* v0,__global float4* v1,__global float4* v2) +{ + *v0 = (float4)(0. ,-vecIn.z ,vecIn.y,0.f); + *v1 = (float4)(vecIn.z ,0. ,-vecIn.x,0.f); + *v2 = (float4)(-vecIn.y ,vecIn.x ,0.f,0.f); +} + + +void getInfo2Point2Point(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies) +{ + float4 posA = bodies[constraint->m_rbA].m_pos; + Quaternion rotA = bodies[constraint->m_rbA].m_quat; + + float4 posB = bodies[constraint->m_rbB].m_pos; + Quaternion rotB = bodies[constraint->m_rbB].m_quat; + + + + // 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; + + float4 a1 = qtRotate(rotA,constraint->m_pivotInA); + + { + __global float4* angular0 = (__global float4*)(info->m_J1angularAxis); + __global float4* angular1 = (__global float4*)(info->m_J1angularAxis+info->rowskip); + __global float4* angular2 = (__global float4*)(info->m_J1angularAxis+2*info->rowskip); + float4 a1neg = -a1; + getSkewSymmetricMatrix(a1neg,angular0,angular1,angular2); + } + if (info->m_J2linearAxis) + { + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip+1] = -1; + info->m_J2linearAxis[2*info->rowskip+2] = -1; + } + + float4 a2 = qtRotate(rotB,constraint->m_pivotInB); + + { + // float4 a2n = -a2; + __global float4* angular0 = (__global float4*)(info->m_J2angularAxis); + __global float4* angular1 = (__global float4*)(info->m_J2angularAxis+info->rowskip); + __global float4* angular2 = (__global float4*)(info->m_J2angularAxis+2*info->rowskip); + getSkewSymmetricMatrix(a2,angular0,angular1,angular2); + } + + // set right hand side +// float currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp; + float currERP = info->erp; + + float k = info->fps * currERP; + int j; + float4 result = a2 + posB - a1 - posA; + float* resultPtr = &result; + + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = k * (resultPtr[j]); + } +} + +Quaternion nearest( Quaternion first, Quaternion qd) +{ + Quaternion diff,sum; + diff = first- qd; + sum = first + qd; + + if( dot(diff,diff) < dot(sum,sum) ) + return qd; + return (-qd); +} + +float b3Acos(float x) +{ + if (x<-1) + x=-1; + if (x>1) + x=1; + return acos(x); +} + +float getAngle(Quaternion orn) +{ + if (orn.w>=1.f) + orn.w=1.f; + float s = 2.f * b3Acos(orn.w); + return s; +} + +void calculateDiffAxisAngleQuaternion( Quaternion orn0,Quaternion orn1a,float4* axis,float* angle) +{ + Quaternion orn1 = nearest(orn0,orn1a); + + Quaternion dorn = qtMul(orn1,qtInvert(orn0)); + *angle = getAngle(dorn); + *axis = (float4)(dorn.x,dorn.y,dorn.z,0.f); + + //check for axis length + float len = dot3F4(*axis,*axis); + if (len < FLT_EPSILON*FLT_EPSILON) + *axis = (float4)(1,0,0,0); + else + *axis /= sqrt(len); +} + + + +void getInfo2FixedOrientation(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies, int start_row) +{ + Quaternion worldOrnA = bodies[constraint->m_rbA].m_quat; + Quaternion worldOrnB = bodies[constraint->m_rbB].m_quat; + + int s = info->rowskip; + int start_index = start_row * s; + + // 3 rows to make body rotations equal + info->m_J1angularAxis[start_index] = 1; + info->m_J1angularAxis[start_index + s + 1] = 1; + info->m_J1angularAxis[start_index + s*2+2] = 1; + if ( info->m_J2angularAxis) + { + info->m_J2angularAxis[start_index] = -1; + info->m_J2angularAxis[start_index + s+1] = -1; + info->m_J2angularAxis[start_index + s*2+2] = -1; + } + + float currERP = info->erp; + float k = info->fps * currERP; + float4 diff; + float angle; + float4 qrelCur = qtMul(worldOrnA,qtInvert(worldOrnB)); + + calculateDiffAxisAngleQuaternion(constraint->m_relTargetAB,qrelCur,&diff,&angle); + diff*=-angle; + + float* resultPtr = &diff; + + for (int j=0; j<3; j++) + { + info->m_constraintError[(3+j)*info->rowskip] = k * resultPtr[j]; + } + + +} + + +__kernel void writeBackVelocitiesKernel(__global b3RigidBodyCL* bodies,__global b3GpuSolverBody* solverBodies,int numBodies) +{ + int i = get_global_id(0); + if (i>=numBodies) + return; + + if (bodies[i].m_invMass) + { +// if (length(solverBodies[i].m_deltaLinearVelocity)=numConstraints) + return; + + //for now, always initialize the batch info + int info1 = infos[i]; + + __global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[constraintRowOffsets[i]]; + __global b3GpuGenericConstraint* constraint = &constraints[i]; + + __global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA]; + __global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB]; + + int solverBodyIdA = constraint->m_rbA; + int solverBodyIdB = constraint->m_rbB; + + __global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA]; + __global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB]; + + + if (rbA->m_invMass) + { + batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA; + } else + { +// if (!solverBodyIdA) +// m_staticIdx = 0; + batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA; + } + + if (rbB->m_invMass) + { + batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB; + } else + { +// if (!solverBodyIdB) +// m_staticIdx = 0; + batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB; + } + + if (info1) + { + int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; +// if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) + // m_maxOverrideNumSolverIterations = overrideNumSolverIterations; + + + int j; + for ( j=0;jm_deltaLinearVelocity = (float4)(0,0,0,0); + bodyAPtr->m_deltaAngularVelocity = (float4)(0,0,0,0); + bodyAPtr->m_pushVelocity = (float4)(0,0,0,0); + bodyAPtr->m_turnVelocity = (float4)(0,0,0,0); + bodyBPtr->m_deltaLinearVelocity = (float4)(0,0,0,0); + bodyBPtr->m_deltaAngularVelocity = (float4)(0,0,0,0); + bodyBPtr->m_pushVelocity = (float4)(0,0,0,0); + bodyBPtr->m_turnVelocity = (float4)(0,0,0,0); + + int rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this + + + + + b3GpuConstraintInfo2 info2; + info2.fps = 1.f/timeStep; + info2.erp = globalErp; + info2.m_J1linearAxisFloat4 = ¤tConstraintRow->m_contactNormal; + info2.m_J1angularAxisFloat4 = ¤tConstraintRow->m_relpos1CrossNormal; + info2.m_J2linearAxisFloat4 = 0; + info2.m_J2angularAxisFloat4 = ¤tConstraintRow->m_relpos2CrossNormal; + info2.rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this + + ///the size of b3SolverConstraint needs be a multiple of float +// b3Assert(info2.rowskip*sizeof(float)== sizeof(b3SolverConstraint)); + info2.m_constraintError = ¤tConstraintRow->m_rhs; + currentConstraintRow->m_cfm = globalCfm; + info2.m_damping = globalDamping; + info2.cfm = ¤tConstraintRow->m_cfm; + info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; + info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + info2.m_numIterations = globalNumIterations; + + switch (constraint->m_constraintType) + { + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + { + getInfo2Point2Point(constraint,&info2,bodies); + break; + } + case B3_GPU_FIXED_CONSTRAINT_TYPE: + { + getInfo2Point2Point(constraint,&info2,bodies); + + getInfo2FixedOrientation(constraint,&info2,bodies,3); + + break; + } + + default: + { + } + } + + ///finalize the constraint setup + for ( j=0;jm_upperLimit>=constraint->m_breakingImpulseThreshold) + { + solverConstraint->m_upperLimit = constraint->m_breakingImpulseThreshold; + } + + if (solverConstraint->m_lowerLimit<=-constraint->m_breakingImpulseThreshold) + { + solverConstraint->m_lowerLimit = -constraint->m_breakingImpulseThreshold; + } + +// solverConstraint->m_originalContactPoint = constraint; + + Matrix3x3 invInertiaWorldA= inertias[constraint->m_rbA].m_invInertiaWorld; + { + + //float4 angularFactorA(1,1,1); + float4 ftorqueAxis1 = solverConstraint->m_relpos1CrossNormal; + solverConstraint->m_angularComponentA = mtMul1(invInertiaWorldA,ftorqueAxis1);//*angularFactorA; + } + + Matrix3x3 invInertiaWorldB= inertias[constraint->m_rbB].m_invInertiaWorld; + { + + float4 ftorqueAxis2 = solverConstraint->m_relpos2CrossNormal; + solverConstraint->m_angularComponentB = mtMul1(invInertiaWorldB,ftorqueAxis2);//*constraint->m_rbB.getAngularFactor(); + } + + { + //it is ok to use solverConstraint->m_contactNormal instead of -solverConstraint->m_contactNormal + //because it gets multiplied iMJlB + float4 iMJlA = solverConstraint->m_contactNormal*rbA->m_invMass; + float4 iMJaA = mtMul3(solverConstraint->m_relpos1CrossNormal,invInertiaWorldA); + float4 iMJlB = solverConstraint->m_contactNormal*rbB->m_invMass;//sign of normal? + float4 iMJaB = mtMul3(solverConstraint->m_relpos2CrossNormal,invInertiaWorldB); + + float sum = dot3F4(iMJlA,solverConstraint->m_contactNormal); + sum += dot3F4(iMJaA,solverConstraint->m_relpos1CrossNormal); + sum += dot3F4(iMJlB,solverConstraint->m_contactNormal); + sum += dot3F4(iMJaB,solverConstraint->m_relpos2CrossNormal); + float fsum = fabs(sum); + if (fsum>FLT_EPSILON) + { + solverConstraint->m_jacDiagABInv = 1.f/sum; + } else + { + solverConstraint->m_jacDiagABInv = 0.f; + } + } + + + ///fix rhs + ///todo: add force/torque accelerators + { + float rel_vel; + float vel1Dotn = dot3F4(solverConstraint->m_contactNormal,rbA->m_linVel) + dot3F4(solverConstraint->m_relpos1CrossNormal,rbA->m_angVel); + float vel2Dotn = -dot3F4(solverConstraint->m_contactNormal,rbB->m_linVel) + dot3F4(solverConstraint->m_relpos2CrossNormal,rbB->m_angVel); + + rel_vel = vel1Dotn+vel2Dotn; + + float restitution = 0.f; + float positionalError = solverConstraint->m_rhs;//already filled in by getConstraintInfo2 + float velocityError = restitution - rel_vel * info2.m_damping; + float penetrationImpulse = positionalError*solverConstraint->m_jacDiagABInv; + float velocityImpulse = velocityError *solverConstraint->m_jacDiagABInv; + solverConstraint->m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint->m_appliedImpulse = 0.f; + + } + } + } +} diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h new file mode 100644 index 000000000000..d48ecf6ea660 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h @@ -0,0 +1,721 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* solveConstraintRowsCL= \ +"/*\n" +"Copyright (c) 2013 Advanced Micro Devices, Inc. \n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose, \n" +"including commercial applications, and to alter it and redistribute it freely, \n" +"subject to the following restrictions:\n" +"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.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"//Originally written by Erwin Coumans\n" +"#define B3_CONSTRAINT_FLAG_ENABLED 1\n" +"#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3\n" +"#define B3_GPU_FIXED_CONSTRAINT_TYPE 4\n" +"#define MOTIONCLAMP 100000 //unused, for debugging/safety in case constraint solver fails\n" +"#define B3_INFINITY 1e30f\n" +"#define mymake_float4 (float4)\n" +"__inline float dot3F4(float4 a, float4 b)\n" +"{\n" +" float4 a1 = mymake_float4(a.xyz,0.f);\n" +" float4 b1 = mymake_float4(b.xyz,0.f);\n" +" return dot(a1, b1);\n" +"}\n" +"typedef float4 Quaternion;\n" +"typedef struct\n" +"{\n" +" float4 m_row[3];\n" +"}Matrix3x3;\n" +"__inline\n" +"float4 mtMul1(Matrix3x3 a, float4 b);\n" +"__inline\n" +"float4 mtMul3(float4 a, Matrix3x3 b);\n" +"__inline\n" +"float4 mtMul1(Matrix3x3 a, float4 b)\n" +"{\n" +" float4 ans;\n" +" ans.x = dot3F4( a.m_row[0], b );\n" +" ans.y = dot3F4( a.m_row[1], b );\n" +" ans.z = dot3F4( a.m_row[2], b );\n" +" ans.w = 0.f;\n" +" return ans;\n" +"}\n" +"__inline\n" +"float4 mtMul3(float4 a, Matrix3x3 b)\n" +"{\n" +" float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" +" float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" +" float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" +" float4 ans;\n" +" ans.x = dot3F4( a, colx );\n" +" ans.y = dot3F4( a, coly );\n" +" ans.z = dot3F4( a, colz );\n" +" return ans;\n" +"}\n" +"typedef struct\n" +"{\n" +" Matrix3x3 m_invInertiaWorld;\n" +" Matrix3x3 m_initInvInertia;\n" +"} BodyInertia;\n" +"typedef struct\n" +"{\n" +" Matrix3x3 m_basis;//orientation\n" +" float4 m_origin;//transform\n" +"}b3Transform;\n" +"typedef struct\n" +"{\n" +"// b3Transform m_worldTransformUnused;\n" +" float4 m_deltaLinearVelocity;\n" +" float4 m_deltaAngularVelocity;\n" +" float4 m_angularFactor;\n" +" float4 m_linearFactor;\n" +" float4 m_invMass;\n" +" float4 m_pushVelocity;\n" +" float4 m_turnVelocity;\n" +" float4 m_linearVelocity;\n" +" float4 m_angularVelocity;\n" +" union \n" +" {\n" +" void* m_originalBody;\n" +" int m_originalBodyIndex;\n" +" };\n" +" int padding[3];\n" +"} b3GpuSolverBody;\n" +"typedef struct\n" +"{\n" +" float4 m_pos;\n" +" Quaternion m_quat;\n" +" float4 m_linVel;\n" +" float4 m_angVel;\n" +" unsigned int m_shapeIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"} b3RigidBodyCL;\n" +"typedef struct\n" +"{\n" +" float4 m_relpos1CrossNormal;\n" +" float4 m_contactNormal;\n" +" float4 m_relpos2CrossNormal;\n" +" //float4 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal\n" +" float4 m_angularComponentA;\n" +" float4 m_angularComponentB;\n" +" \n" +" float m_appliedPushImpulse;\n" +" float m_appliedImpulse;\n" +" int m_padding1;\n" +" int m_padding2;\n" +" float m_friction;\n" +" float m_jacDiagABInv;\n" +" float m_rhs;\n" +" float m_cfm;\n" +" \n" +" float m_lowerLimit;\n" +" float m_upperLimit;\n" +" float m_rhsPenetration;\n" +" int m_originalConstraint;\n" +" int m_overrideNumSolverIterations;\n" +" int m_frictionIndex;\n" +" int m_solverBodyIdA;\n" +" int m_solverBodyIdB;\n" +"} b3SolverConstraint;\n" +"typedef struct \n" +"{\n" +" int m_bodyAPtrAndSignBit;\n" +" int m_bodyBPtrAndSignBit;\n" +" int m_originalConstraintIndex;\n" +" int m_batchId;\n" +"} b3BatchConstraint;\n" +"typedef struct \n" +"{\n" +" int m_constraintType;\n" +" int m_rbA;\n" +" int m_rbB;\n" +" float m_breakingImpulseThreshold;\n" +" float4 m_pivotInA;\n" +" float4 m_pivotInB;\n" +" Quaternion m_relTargetAB;\n" +" int m_flags;\n" +" int m_padding[3];\n" +"} b3GpuGenericConstraint;\n" +"/*b3Transform getWorldTransform(b3RigidBodyCL* rb)\n" +"{\n" +" b3Transform newTrans;\n" +" newTrans.setOrigin(rb->m_pos);\n" +" newTrans.setRotation(rb->m_quat);\n" +" return newTrans;\n" +"}*/\n" +"__inline\n" +"float4 cross3(float4 a, float4 b)\n" +"{\n" +" return cross(a,b);\n" +"}\n" +"__inline\n" +"float4 fastNormalize4(float4 v)\n" +"{\n" +" v = mymake_float4(v.xyz,0.f);\n" +" return fast_normalize(v);\n" +"}\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b);\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in);\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec);\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q);\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b)\n" +"{\n" +" Quaternion ans;\n" +" ans = cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in)\n" +"{\n" +" return fastNormalize4(in);\n" +"// in /= length( in );\n" +"// return in;\n" +"}\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec)\n" +"{\n" +" Quaternion qInv = qtInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q)\n" +"{\n" +" return (Quaternion)(-q.xyz, q.w);\n" +"}\n" +"__inline void internalApplyImpulse(__global b3GpuSolverBody* body, float4 linearComponent, float4 angularComponent,float impulseMagnitude)\n" +"{\n" +" body->m_deltaLinearVelocity += linearComponent*impulseMagnitude*body->m_linearFactor;\n" +" body->m_deltaAngularVelocity += angularComponent*(impulseMagnitude*body->m_angularFactor);\n" +"}\n" +"void resolveSingleConstraintRowGeneric(__global b3GpuSolverBody* body1, __global b3GpuSolverBody* body2, __global b3SolverConstraint* c)\n" +"{\n" +" float deltaImpulse = c->m_rhs-c->m_appliedImpulse*c->m_cfm;\n" +" float deltaVel1Dotn = dot3F4(c->m_contactNormal,body1->m_deltaLinearVelocity) + dot3F4(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity);\n" +" float deltaVel2Dotn = -dot3F4(c->m_contactNormal,body2->m_deltaLinearVelocity) + dot3F4(c->m_relpos2CrossNormal,body2->m_deltaAngularVelocity);\n" +" deltaImpulse -= deltaVel1Dotn*c->m_jacDiagABInv;\n" +" deltaImpulse -= deltaVel2Dotn*c->m_jacDiagABInv;\n" +" float sum = c->m_appliedImpulse + deltaImpulse;\n" +" if (sum < c->m_lowerLimit)\n" +" {\n" +" deltaImpulse = c->m_lowerLimit-c->m_appliedImpulse;\n" +" c->m_appliedImpulse = c->m_lowerLimit;\n" +" }\n" +" else if (sum > c->m_upperLimit) \n" +" {\n" +" deltaImpulse = c->m_upperLimit-c->m_appliedImpulse;\n" +" c->m_appliedImpulse = c->m_upperLimit;\n" +" }\n" +" else\n" +" {\n" +" c->m_appliedImpulse = sum;\n" +" }\n" +" internalApplyImpulse(body1,c->m_contactNormal*body1->m_invMass,c->m_angularComponentA,deltaImpulse);\n" +" internalApplyImpulse(body2,-c->m_contactNormal*body2->m_invMass,c->m_angularComponentB,deltaImpulse);\n" +"}\n" +"__kernel void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,\n" +" __global b3BatchConstraint* batchConstraints,\n" +" __global b3SolverConstraint* rows,\n" +" __global unsigned int* numConstraintRowsInfo1, \n" +" __global unsigned int* rowOffsets,\n" +" __global b3GpuGenericConstraint* constraints,\n" +" int batchOffset,\n" +" int numConstraintsInBatch\n" +" )\n" +"{\n" +" int b = get_global_id(0);\n" +" if (b>=numConstraintsInBatch)\n" +" return;\n" +" __global b3BatchConstraint* c = &batchConstraints[b+batchOffset];\n" +" int originalConstraintIndex = c->m_originalConstraintIndex;\n" +" if (constraints[originalConstraintIndex].m_flags&B3_CONSTRAINT_FLAG_ENABLED)\n" +" {\n" +" int numConstraintRows = numConstraintRowsInfo1[originalConstraintIndex];\n" +" int rowOffset = rowOffsets[originalConstraintIndex];\n" +" for (int jj=0;jjm_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint);\n" +" }\n" +" }\n" +"};\n" +"__kernel void initSolverBodies(__global b3GpuSolverBody* solverBodies,__global b3RigidBodyCL* bodiesCL, int numBodies)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numBodies)\n" +" return;\n" +" __global b3GpuSolverBody* solverBody = &solverBodies[i];\n" +" __global b3RigidBodyCL* bodyCL = &bodiesCL[i];\n" +" solverBody->m_deltaLinearVelocity = (float4)(0.f,0.f,0.f,0.f);\n" +" solverBody->m_deltaAngularVelocity = (float4)(0.f,0.f,0.f,0.f);\n" +" solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f);\n" +" solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f);\n" +" solverBody->m_invMass = (float4)(bodyCL->m_invMass,bodyCL->m_invMass,bodyCL->m_invMass,0.f);\n" +" solverBody->m_originalBodyIndex = i;\n" +" solverBody->m_angularFactor = (float4)(1,1,1,0);\n" +" solverBody->m_linearFactor = (float4) (1,1,1,0);\n" +" solverBody->m_linearVelocity = bodyCL->m_linVel;\n" +" solverBody->m_angularVelocity = bodyCL->m_angVel;\n" +"}\n" +"__kernel void breakViolatedConstraintsKernel(__global b3GpuGenericConstraint* constraints, __global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, __global b3SolverConstraint* rows, int numConstraints)\n" +"{\n" +" int cid = get_global_id(0);\n" +" if (cid>=numConstraints)\n" +" return;\n" +" int numRows = numConstraintRows[cid];\n" +" if (numRows)\n" +" {\n" +" for (int i=0;i= breakingThreshold)\n" +" {\n" +" constraints[cid].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED;\n" +" }\n" +" }\n" +" }\n" +"}\n" +"__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, int numConstraints)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numConstraints)\n" +" return;\n" +" __global b3GpuGenericConstraint* constraint = &constraints[i];\n" +" switch (constraint->m_constraintType)\n" +" {\n" +" case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n" +" {\n" +" infos[i] = 3;\n" +" break;\n" +" }\n" +" case B3_GPU_FIXED_CONSTRAINT_TYPE:\n" +" {\n" +" infos[i] = 6;\n" +" break;\n" +" }\n" +" default:\n" +" {\n" +" }\n" +" }\n" +"}\n" +"__kernel void initBatchConstraintsKernel(__global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, \n" +" __global b3BatchConstraint* batchConstraints, \n" +" __global b3GpuGenericConstraint* constraints,\n" +" __global b3RigidBodyCL* bodies,\n" +" int numConstraints)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numConstraints)\n" +" return;\n" +" int rbA = constraints[i].m_rbA;\n" +" int rbB = constraints[i].m_rbB;\n" +" batchConstraints[i].m_bodyAPtrAndSignBit = bodies[rbA].m_invMass != 0.f ? rbA : -rbA;\n" +" batchConstraints[i].m_bodyBPtrAndSignBit = bodies[rbB].m_invMass != 0.f ? rbB : -rbB;\n" +" batchConstraints[i].m_batchId = -1;\n" +" batchConstraints[i].m_originalConstraintIndex = i;\n" +"}\n" +"typedef struct\n" +"{\n" +" // integrator parameters: frames per second (1/stepsize), default error\n" +" // reduction parameter (0..1).\n" +" float fps,erp;\n" +" // for the first and second body, pointers to two (linear and angular)\n" +" // n*3 jacobian sub matrices, stored by rows. these matrices will have\n" +" // been initialized to 0 on entry. if the second body is zero then the\n" +" // J2xx pointers may be 0.\n" +" union \n" +" {\n" +" __global float4* m_J1linearAxisFloat4;\n" +" __global float* m_J1linearAxis;\n" +" };\n" +" union\n" +" {\n" +" __global float4* m_J1angularAxisFloat4;\n" +" __global float* m_J1angularAxis;\n" +" };\n" +" union\n" +" {\n" +" __global float4* m_J2linearAxisFloat4;\n" +" __global float* m_J2linearAxis;\n" +" };\n" +" union\n" +" {\n" +" __global float4* m_J2angularAxisFloat4;\n" +" __global float* m_J2angularAxis;\n" +" };\n" +" // elements to jump from one row to the next in J's\n" +" int rowskip;\n" +" // right hand sides of the equation J*v = c + cfm * lambda. cfm is the\n" +" // \"constraint force mixing\" vector. c is set to zero on entry, cfm is\n" +" // set to a constant value (typically very small or zero) value on entry.\n" +" __global float* m_constraintError;\n" +" __global float* cfm;\n" +" // lo and hi limits for variables (set to -/+ infinity on entry).\n" +" __global float* m_lowerLimit;\n" +" __global float* m_upperLimit;\n" +" // findex vector for variables. see the LCP solver interface for a\n" +" // description of what this does. this is set to -1 on entry.\n" +" // note that the returned indexes are relative to the first index of\n" +" // the constraint.\n" +" __global int *findex;\n" +" // number of solver iterations\n" +" int m_numIterations;\n" +" //damping of the velocity\n" +" float m_damping;\n" +"} b3GpuConstraintInfo2;\n" +"void getSkewSymmetricMatrix(float4 vecIn, __global float4* v0,__global float4* v1,__global float4* v2)\n" +"{\n" +" *v0 = (float4)(0. ,-vecIn.z ,vecIn.y,0.f);\n" +" *v1 = (float4)(vecIn.z ,0. ,-vecIn.x,0.f);\n" +" *v2 = (float4)(-vecIn.y ,vecIn.x ,0.f,0.f);\n" +"}\n" +"void getInfo2Point2Point(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies)\n" +"{\n" +" float4 posA = bodies[constraint->m_rbA].m_pos;\n" +" Quaternion rotA = bodies[constraint->m_rbA].m_quat;\n" +" float4 posB = bodies[constraint->m_rbB].m_pos;\n" +" Quaternion rotB = bodies[constraint->m_rbB].m_quat;\n" +" // anchor points in global coordinates with respect to body PORs.\n" +" \n" +" // set jacobian\n" +" info->m_J1linearAxis[0] = 1;\n" +" info->m_J1linearAxis[info->rowskip+1] = 1;\n" +" info->m_J1linearAxis[2*info->rowskip+2] = 1;\n" +" float4 a1 = qtRotate(rotA,constraint->m_pivotInA);\n" +" {\n" +" __global float4* angular0 = (__global float4*)(info->m_J1angularAxis);\n" +" __global float4* angular1 = (__global float4*)(info->m_J1angularAxis+info->rowskip);\n" +" __global float4* angular2 = (__global float4*)(info->m_J1angularAxis+2*info->rowskip);\n" +" float4 a1neg = -a1;\n" +" getSkewSymmetricMatrix(a1neg,angular0,angular1,angular2);\n" +" }\n" +" if (info->m_J2linearAxis)\n" +" {\n" +" info->m_J2linearAxis[0] = -1;\n" +" info->m_J2linearAxis[info->rowskip+1] = -1;\n" +" info->m_J2linearAxis[2*info->rowskip+2] = -1;\n" +" }\n" +" \n" +" float4 a2 = qtRotate(rotB,constraint->m_pivotInB);\n" +" \n" +" {\n" +" // float4 a2n = -a2;\n" +" __global float4* angular0 = (__global float4*)(info->m_J2angularAxis);\n" +" __global float4* angular1 = (__global float4*)(info->m_J2angularAxis+info->rowskip);\n" +" __global float4* angular2 = (__global float4*)(info->m_J2angularAxis+2*info->rowskip);\n" +" getSkewSymmetricMatrix(a2,angular0,angular1,angular2);\n" +" }\n" +" \n" +" // set right hand side\n" +"// float currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;\n" +" float currERP = info->erp;\n" +" float k = info->fps * currERP;\n" +" int j;\n" +" float4 result = a2 + posB - a1 - posA;\n" +" float* resultPtr = &result;\n" +" for (j=0; j<3; j++)\n" +" {\n" +" info->m_constraintError[j*info->rowskip] = k * (resultPtr[j]);\n" +" }\n" +"}\n" +"Quaternion nearest( Quaternion first, Quaternion qd)\n" +"{\n" +" Quaternion diff,sum;\n" +" diff = first- qd;\n" +" sum = first + qd;\n" +" \n" +" if( dot(diff,diff) < dot(sum,sum) )\n" +" return qd;\n" +" return (-qd);\n" +"}\n" +"float b3Acos(float x) \n" +"{ \n" +" if (x<-1) \n" +" x=-1; \n" +" if (x>1) \n" +" x=1;\n" +" return acos(x); \n" +"}\n" +"float getAngle(Quaternion orn)\n" +"{\n" +" if (orn.w>=1.f)\n" +" orn.w=1.f;\n" +" float s = 2.f * b3Acos(orn.w);\n" +" return s;\n" +"}\n" +"void calculateDiffAxisAngleQuaternion( Quaternion orn0,Quaternion orn1a,float4* axis,float* angle)\n" +"{\n" +" Quaternion orn1 = nearest(orn0,orn1a);\n" +" \n" +" Quaternion dorn = qtMul(orn1,qtInvert(orn0));\n" +" *angle = getAngle(dorn);\n" +" *axis = (float4)(dorn.x,dorn.y,dorn.z,0.f);\n" +" \n" +" //check for axis length\n" +" float len = dot3F4(*axis,*axis);\n" +" if (len < FLT_EPSILON*FLT_EPSILON)\n" +" *axis = (float4)(1,0,0,0);\n" +" else\n" +" *axis /= sqrt(len);\n" +"}\n" +"void getInfo2FixedOrientation(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies, int start_row)\n" +"{\n" +" Quaternion worldOrnA = bodies[constraint->m_rbA].m_quat;\n" +" Quaternion worldOrnB = bodies[constraint->m_rbB].m_quat;\n" +" int s = info->rowskip;\n" +" int start_index = start_row * s;\n" +" // 3 rows to make body rotations equal\n" +" info->m_J1angularAxis[start_index] = 1;\n" +" info->m_J1angularAxis[start_index + s + 1] = 1;\n" +" info->m_J1angularAxis[start_index + s*2+2] = 1;\n" +" if ( info->m_J2angularAxis)\n" +" {\n" +" info->m_J2angularAxis[start_index] = -1;\n" +" info->m_J2angularAxis[start_index + s+1] = -1;\n" +" info->m_J2angularAxis[start_index + s*2+2] = -1;\n" +" }\n" +" \n" +" float currERP = info->erp;\n" +" float k = info->fps * currERP;\n" +" float4 diff;\n" +" float angle;\n" +" float4 qrelCur = qtMul(worldOrnA,qtInvert(worldOrnB));\n" +" \n" +" calculateDiffAxisAngleQuaternion(constraint->m_relTargetAB,qrelCur,&diff,&angle);\n" +" diff*=-angle;\n" +" \n" +" float* resultPtr = &diff;\n" +" \n" +" for (int j=0; j<3; j++)\n" +" {\n" +" info->m_constraintError[(3+j)*info->rowskip] = k * resultPtr[j];\n" +" }\n" +" \n" +"}\n" +"__kernel void writeBackVelocitiesKernel(__global b3RigidBodyCL* bodies,__global b3GpuSolverBody* solverBodies,int numBodies)\n" +"{\n" +" int i = get_global_id(0);\n" +" if (i>=numBodies)\n" +" return;\n" +" if (bodies[i].m_invMass)\n" +" {\n" +"// if (length(solverBodies[i].m_deltaLinearVelocity)=numConstraints)\n" +" return;\n" +" \n" +" //for now, always initialize the batch info\n" +" int info1 = infos[i];\n" +" \n" +" __global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[constraintRowOffsets[i]];\n" +" __global b3GpuGenericConstraint* constraint = &constraints[i];\n" +" __global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA];\n" +" __global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB];\n" +" int solverBodyIdA = constraint->m_rbA;\n" +" int solverBodyIdB = constraint->m_rbB;\n" +" __global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA];\n" +" __global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB];\n" +" if (rbA->m_invMass)\n" +" {\n" +" batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA;\n" +" } else\n" +" {\n" +"// if (!solverBodyIdA)\n" +"// m_staticIdx = 0;\n" +" batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA;\n" +" }\n" +" if (rbB->m_invMass)\n" +" {\n" +" batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB;\n" +" } else\n" +" {\n" +"// if (!solverBodyIdB)\n" +"// m_staticIdx = 0;\n" +" batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB;\n" +" }\n" +" if (info1)\n" +" {\n" +" int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;\n" +"// if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)\n" +" // m_maxOverrideNumSolverIterations = overrideNumSolverIterations;\n" +" int j;\n" +" for ( j=0;jm_deltaLinearVelocity = (float4)(0,0,0,0);\n" +" bodyAPtr->m_deltaAngularVelocity = (float4)(0,0,0,0);\n" +" bodyAPtr->m_pushVelocity = (float4)(0,0,0,0);\n" +" bodyAPtr->m_turnVelocity = (float4)(0,0,0,0);\n" +" bodyBPtr->m_deltaLinearVelocity = (float4)(0,0,0,0);\n" +" bodyBPtr->m_deltaAngularVelocity = (float4)(0,0,0,0);\n" +" bodyBPtr->m_pushVelocity = (float4)(0,0,0,0);\n" +" bodyBPtr->m_turnVelocity = (float4)(0,0,0,0);\n" +" int rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n" +" \n" +" b3GpuConstraintInfo2 info2;\n" +" info2.fps = 1.f/timeStep;\n" +" info2.erp = globalErp;\n" +" info2.m_J1linearAxisFloat4 = ¤tConstraintRow->m_contactNormal;\n" +" info2.m_J1angularAxisFloat4 = ¤tConstraintRow->m_relpos1CrossNormal;\n" +" info2.m_J2linearAxisFloat4 = 0;\n" +" info2.m_J2angularAxisFloat4 = ¤tConstraintRow->m_relpos2CrossNormal;\n" +" info2.rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n" +" ///the size of b3SolverConstraint needs be a multiple of float\n" +"// b3Assert(info2.rowskip*sizeof(float)== sizeof(b3SolverConstraint));\n" +" info2.m_constraintError = ¤tConstraintRow->m_rhs;\n" +" currentConstraintRow->m_cfm = globalCfm;\n" +" info2.m_damping = globalDamping;\n" +" info2.cfm = ¤tConstraintRow->m_cfm;\n" +" info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;\n" +" info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;\n" +" info2.m_numIterations = globalNumIterations;\n" +" switch (constraint->m_constraintType)\n" +" {\n" +" case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n" +" {\n" +" getInfo2Point2Point(constraint,&info2,bodies);\n" +" break;\n" +" }\n" +" case B3_GPU_FIXED_CONSTRAINT_TYPE:\n" +" {\n" +" getInfo2Point2Point(constraint,&info2,bodies);\n" +" getInfo2FixedOrientation(constraint,&info2,bodies,3);\n" +" break;\n" +" }\n" +" default:\n" +" {\n" +" }\n" +" }\n" +" ///finalize the constraint setup\n" +" for ( j=0;jm_upperLimit>=constraint->m_breakingImpulseThreshold)\n" +" {\n" +" solverConstraint->m_upperLimit = constraint->m_breakingImpulseThreshold;\n" +" }\n" +" if (solverConstraint->m_lowerLimit<=-constraint->m_breakingImpulseThreshold)\n" +" {\n" +" solverConstraint->m_lowerLimit = -constraint->m_breakingImpulseThreshold;\n" +" }\n" +"// solverConstraint->m_originalContactPoint = constraint;\n" +" \n" +" Matrix3x3 invInertiaWorldA= inertias[constraint->m_rbA].m_invInertiaWorld;\n" +" {\n" +" //float4 angularFactorA(1,1,1);\n" +" float4 ftorqueAxis1 = solverConstraint->m_relpos1CrossNormal;\n" +" solverConstraint->m_angularComponentA = mtMul1(invInertiaWorldA,ftorqueAxis1);//*angularFactorA;\n" +" }\n" +" \n" +" Matrix3x3 invInertiaWorldB= inertias[constraint->m_rbB].m_invInertiaWorld;\n" +" {\n" +" float4 ftorqueAxis2 = solverConstraint->m_relpos2CrossNormal;\n" +" solverConstraint->m_angularComponentB = mtMul1(invInertiaWorldB,ftorqueAxis2);//*constraint->m_rbB.getAngularFactor();\n" +" }\n" +" {\n" +" //it is ok to use solverConstraint->m_contactNormal instead of -solverConstraint->m_contactNormal\n" +" //because it gets multiplied iMJlB\n" +" float4 iMJlA = solverConstraint->m_contactNormal*rbA->m_invMass;\n" +" float4 iMJaA = mtMul3(solverConstraint->m_relpos1CrossNormal,invInertiaWorldA);\n" +" float4 iMJlB = solverConstraint->m_contactNormal*rbB->m_invMass;//sign of normal?\n" +" float4 iMJaB = mtMul3(solverConstraint->m_relpos2CrossNormal,invInertiaWorldB);\n" +" float sum = dot3F4(iMJlA,solverConstraint->m_contactNormal);\n" +" sum += dot3F4(iMJaA,solverConstraint->m_relpos1CrossNormal);\n" +" sum += dot3F4(iMJlB,solverConstraint->m_contactNormal);\n" +" sum += dot3F4(iMJaB,solverConstraint->m_relpos2CrossNormal);\n" +" float fsum = fabs(sum);\n" +" if (fsum>FLT_EPSILON)\n" +" {\n" +" solverConstraint->m_jacDiagABInv = 1.f/sum;\n" +" } else\n" +" {\n" +" solverConstraint->m_jacDiagABInv = 0.f;\n" +" }\n" +" }\n" +" ///fix rhs\n" +" ///todo: add force/torque accelerators\n" +" {\n" +" float rel_vel;\n" +" float vel1Dotn = dot3F4(solverConstraint->m_contactNormal,rbA->m_linVel) + dot3F4(solverConstraint->m_relpos1CrossNormal,rbA->m_angVel);\n" +" float vel2Dotn = -dot3F4(solverConstraint->m_contactNormal,rbB->m_linVel) + dot3F4(solverConstraint->m_relpos2CrossNormal,rbB->m_angVel);\n" +" rel_vel = vel1Dotn+vel2Dotn;\n" +" float restitution = 0.f;\n" +" float positionalError = solverConstraint->m_rhs;//already filled in by getConstraintInfo2\n" +" float velocityError = restitution - rel_vel * info2.m_damping;\n" +" float penetrationImpulse = positionalError*solverConstraint->m_jacDiagABInv;\n" +" float velocityImpulse = velocityError *solverConstraint->m_jacDiagABInv;\n" +" solverConstraint->m_rhs = penetrationImpulse+velocityImpulse;\n" +" solverConstraint->m_appliedImpulse = 0.f;\n" +" }\n" +" }\n" +" }\n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl new file mode 100644 index 000000000000..5c4d62e4ec93 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl @@ -0,0 +1,501 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, 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. +*/ +//Originally written by Takahiro Harada + + +//#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable + + +#ifdef cl_ext_atomic_counters_32 +#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable +#else +#define counter32_t volatile global int* +#endif + +typedef unsigned int u32; +typedef unsigned short u16; +typedef unsigned char u8; + +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GET_NUM_GROUPS get_num_groups(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) +#define AppendInc(x, out) out = atomic_inc(x) +#define AtomAdd(x, value) atom_add(&(x), value) +#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value ) +#define AtomXhg(x, value) atom_xchg ( &(x), value ) + + +#define SELECT_UINT4( b, a, condition ) select( b,a,condition ) + +#define mymake_float4 (float4) +//#define make_float2 (float2) +//#define make_uint4 (uint4) +//#define make_int4 (int4) +//#define make_uint2 (uint2) +//#define make_int2 (int2) + + +#define max2 max +#define min2 min + + +/////////////////////////////////////// +// Vector +/////////////////////////////////////// + + + + +__inline +float4 fastNormalize4(float4 v) +{ + return fast_normalize(v); +} + + + +__inline +float4 cross3(float4 a, float4 b) +{ + return cross(a,b); +} + +__inline +float dot3F4(float4 a, float4 b) +{ + float4 a1 = mymake_float4(a.xyz,0.f); + float4 b1 = mymake_float4(b.xyz,0.f); + return dot(a1, b1); +} + + + + +__inline +float4 normalize3(const float4 a) +{ + float4 n = mymake_float4(a.x, a.y, a.z, 0.f); + return fastNormalize4( n ); +// float length = sqrtf(dot3F4(a, a)); +// return 1.f/length * a; +} + + + + +/////////////////////////////////////// +// Matrix3x3 +/////////////////////////////////////// + +typedef struct +{ + float4 m_row[3]; +}Matrix3x3; + + + + + + +__inline +float4 mtMul1(Matrix3x3 a, float4 b); + +__inline +float4 mtMul3(float4 a, Matrix3x3 b); + + + + +__inline +float4 mtMul1(Matrix3x3 a, float4 b) +{ + float4 ans; + ans.x = dot3F4( a.m_row[0], b ); + ans.y = dot3F4( a.m_row[1], b ); + ans.z = dot3F4( a.m_row[2], b ); + ans.w = 0.f; + return ans; +} + +__inline +float4 mtMul3(float4 a, Matrix3x3 b) +{ + float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0); + float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0); + float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0); + + float4 ans; + ans.x = dot3F4( a, colx ); + ans.y = dot3F4( a, coly ); + ans.z = dot3F4( a, colz ); + return ans; +} + +/////////////////////////////////////// +// Quaternion +/////////////////////////////////////// + +typedef float4 Quaternion; + + + + + + + +#define WG_SIZE 64 + +typedef struct +{ + float4 m_pos; + Quaternion m_quat; + float4 m_linVel; + float4 m_angVel; + + u32 m_shapeIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} Body; + +typedef struct +{ + Matrix3x3 m_invInertia; + Matrix3x3 m_initInvInertia; +} Shape; + +typedef struct +{ + float4 m_linear; + float4 m_worldPos[4]; + float4 m_center; + float m_jacCoeffInv[4]; + float m_b[4]; + float m_appliedRambdaDt[4]; + + float m_fJacCoeffInv[2]; + float m_fAppliedRambdaDt[2]; + + u32 m_bodyA; + u32 m_bodyB; + + int m_batchIdx; + u32 m_paddings[1]; +} Constraint4; + + + +typedef struct +{ + int m_nConstraints; + int m_start; + int m_batchIdx; + int m_nSplit; +// int m_paddings[1]; +} ConstBuffer; + +typedef struct +{ + int m_solveFriction; + int m_maxBatch; // long batch really kills the performance + int m_batchIdx; + int m_nSplit; +// int m_paddings[1]; +} ConstBufferBatchSolve; + +void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1); + +void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1) +{ + *linear = mymake_float4(-n.xyz,0.f); + *angular0 = -cross3(r0, n); + *angular1 = cross3(r1, n); +} + +float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 ); + +float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 ) +{ + return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1); +} + + +float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1, + float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1); + +float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1, + float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1) +{ + // linear0,1 are normlized + float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0; + float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0); + float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1; + float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1); + return -1.f/(jmj0+jmj1+jmj2+jmj3); +} + + +void solveContact(__global Constraint4* cs, + float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA, + float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB); + +void solveContact(__global Constraint4* cs, + float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA, + float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB) +{ + float minRambdaDt = 0; + float maxRambdaDt = FLT_MAX; + + for(int ic=0; ic<4; ic++) + { + if( cs->m_jacCoeffInv[ic] == 0.f ) continue; + + float4 angular0, angular1, linear; + float4 r0 = cs->m_worldPos[ic] - posA; + float4 r1 = cs->m_worldPos[ic] - posB; + setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 ); + + float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, + *linVelA, *angVelA, *linVelB, *angVelB ) + cs->m_b[ic]; + rambdaDt *= cs->m_jacCoeffInv[ic]; + + { + float prevSum = cs->m_appliedRambdaDt[ic]; + float updated = prevSum; + updated += rambdaDt; + updated = max2( updated, minRambdaDt ); + updated = min2( updated, maxRambdaDt ); + rambdaDt = updated - prevSum; + cs->m_appliedRambdaDt[ic] = updated; + } + + float4 linImp0 = invMassA*linear*rambdaDt; + float4 linImp1 = invMassB*(-linear)*rambdaDt; + float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt; + float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt; + + *linVelA += linImp0; + *angVelA += angImp0; + *linVelB += linImp1; + *angVelB += angImp1; + } +} + +void btPlaneSpace1 (const float4* n, float4* p, float4* q); + void btPlaneSpace1 (const float4* n, float4* p, float4* q) +{ + if (fabs(n[0].z) > 0.70710678f) { + // choose p in y-z plane + float a = n[0].y*n[0].y + n[0].z*n[0].z; + float k = 1.f/sqrt(a); + p[0].x = 0; + p[0].y = -n[0].z*k; + p[0].z = n[0].y*k; + // set q = n x p + q[0].x = a*k; + q[0].y = -n[0].x*p[0].z; + q[0].z = n[0].x*p[0].y; + } + else { + // choose p in x-y plane + float a = n[0].x*n[0].x + n[0].y*n[0].y; + float k = 1.f/sqrt(a); + p[0].x = -n[0].y*k; + p[0].y = n[0].x*k; + p[0].z = 0; + // set q = n x p + q[0].x = -n[0].z*p[0].y; + q[0].y = n[0].z*p[0].x; + q[0].z = a*k; + } +} + +void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs); +void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs) +{ + //float frictionCoeff = ldsCs[0].m_linear.w; + int aIdx = ldsCs[0].m_bodyA; + int bIdx = ldsCs[0].m_bodyB; + + float4 posA = gBodies[aIdx].m_pos; + float4 linVelA = gBodies[aIdx].m_linVel; + float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia; + + float4 posB = gBodies[bIdx].m_pos; + float4 linVelB = gBodies[bIdx].m_linVel; + float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia; + + solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA, + posB, &linVelB, &angVelB, invMassB, invInertiaB ); + + if (gBodies[aIdx].m_invMass) + { + gBodies[aIdx].m_linVel = linVelA; + gBodies[aIdx].m_angVel = angVelA; + } else + { + gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0); + gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0); + + } + if (gBodies[bIdx].m_invMass) + { + gBodies[bIdx].m_linVel = linVelB; + gBodies[bIdx].m_angVel = angVelB; + } else + { + gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0); + gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0); + + } + +} + + + +typedef struct +{ + int m_valInt0; + int m_valInt1; + int m_valInt2; + int m_valInt3; + + float m_val0; + float m_val1; + float m_val2; + float m_val3; +} SolverDebugInfo; + + + + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void BatchSolveKernelContact(__global Body* gBodies, + __global Shape* gShapes, + __global Constraint4* gConstraints, + __global int* gN, + __global int* gOffsets, + __global int* batchSizes, + int maxBatch1, + int cellBatch, + int4 nSplit + ) +{ + //__local int ldsBatchIdx[WG_SIZE+1]; + __local int ldsCurBatch; + __local int ldsNextBatch; + __local int ldsStart; + + int lIdx = GET_LOCAL_IDX; + int wgIdx = GET_GROUP_IDX; + +// int gIdx = GET_GLOBAL_IDX; +// debugInfo[gIdx].m_valInt0 = gIdx; + //debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE; + + + + + int zIdx = (wgIdx/((nSplit.x*nSplit.y)/4))*2+((cellBatch&4)>>2); + int remain= (wgIdx%((nSplit.x*nSplit.y)/4)); + int yIdx = (remain/(nSplit.x/2))*2 + ((cellBatch&2)>>1); + int xIdx = (remain%(nSplit.x/2))*2 + (cellBatch&1); + int cellIdx = xIdx+yIdx*nSplit.x+zIdx*(nSplit.x*nSplit.y); + + //int xIdx = (wgIdx/(nSplit/2))*2 + (bIdx&1); + //int yIdx = (wgIdx%(nSplit/2))*2 + (bIdx>>1); + //int cellIdx = xIdx+yIdx*nSplit; + + if( gN[cellIdx] == 0 ) + return; + + int maxBatch = batchSizes[cellIdx]; + + + const int start = gOffsets[cellIdx]; + const int end = start + gN[cellIdx]; + + + + + if( lIdx == 0 ) + { + ldsCurBatch = 0; + ldsNextBatch = 0; + ldsStart = start; + } + + + GROUP_LDS_BARRIER; + + int idx=ldsStart+lIdx; + while (ldsCurBatch < maxBatch) + { + for(; idxm_jacCoeffInv[ic] == 0.f ) continue;\n" +" float4 angular0, angular1, linear;\n" +" float4 r0 = cs->m_worldPos[ic] - posA;\n" +" float4 r1 = cs->m_worldPos[ic] - posB;\n" +" setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 );\n" +" float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, \n" +" *linVelA, *angVelA, *linVelB, *angVelB ) + cs->m_b[ic];\n" +" rambdaDt *= cs->m_jacCoeffInv[ic];\n" +" {\n" +" float prevSum = cs->m_appliedRambdaDt[ic];\n" +" float updated = prevSum;\n" +" updated += rambdaDt;\n" +" updated = max2( updated, minRambdaDt );\n" +" updated = min2( updated, maxRambdaDt );\n" +" rambdaDt = updated - prevSum;\n" +" cs->m_appliedRambdaDt[ic] = updated;\n" +" }\n" +" float4 linImp0 = invMassA*linear*rambdaDt;\n" +" float4 linImp1 = invMassB*(-linear)*rambdaDt;\n" +" float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n" +" float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n" +" *linVelA += linImp0;\n" +" *angVelA += angImp0;\n" +" *linVelB += linImp1;\n" +" *angVelB += angImp1;\n" +" }\n" +"}\n" +"void btPlaneSpace1 (const float4* n, float4* p, float4* q);\n" +" void btPlaneSpace1 (const float4* n, float4* p, float4* q)\n" +"{\n" +" if (fabs(n[0].z) > 0.70710678f) {\n" +" // choose p in y-z plane\n" +" float a = n[0].y*n[0].y + n[0].z*n[0].z;\n" +" float k = 1.f/sqrt(a);\n" +" p[0].x = 0;\n" +" p[0].y = -n[0].z*k;\n" +" p[0].z = n[0].y*k;\n" +" // set q = n x p\n" +" q[0].x = a*k;\n" +" q[0].y = -n[0].x*p[0].z;\n" +" q[0].z = n[0].x*p[0].y;\n" +" }\n" +" else {\n" +" // choose p in x-y plane\n" +" float a = n[0].x*n[0].x + n[0].y*n[0].y;\n" +" float k = 1.f/sqrt(a);\n" +" p[0].x = -n[0].y*k;\n" +" p[0].y = n[0].x*k;\n" +" p[0].z = 0;\n" +" // set q = n x p\n" +" q[0].x = -n[0].z*p[0].y;\n" +" q[0].y = n[0].z*p[0].x;\n" +" q[0].z = a*k;\n" +" }\n" +"}\n" +"void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs);\n" +"void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs)\n" +"{\n" +" //float frictionCoeff = ldsCs[0].m_linear.w;\n" +" int aIdx = ldsCs[0].m_bodyA;\n" +" int bIdx = ldsCs[0].m_bodyB;\n" +" float4 posA = gBodies[aIdx].m_pos;\n" +" float4 linVelA = gBodies[aIdx].m_linVel;\n" +" float4 angVelA = gBodies[aIdx].m_angVel;\n" +" float invMassA = gBodies[aIdx].m_invMass;\n" +" Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" +" float4 posB = gBodies[bIdx].m_pos;\n" +" float4 linVelB = gBodies[bIdx].m_linVel;\n" +" float4 angVelB = gBodies[bIdx].m_angVel;\n" +" float invMassB = gBodies[bIdx].m_invMass;\n" +" Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" +" solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n" +" posB, &linVelB, &angVelB, invMassB, invInertiaB );\n" +" if (gBodies[aIdx].m_invMass)\n" +" {\n" +" gBodies[aIdx].m_linVel = linVelA;\n" +" gBodies[aIdx].m_angVel = angVelA;\n" +" } else\n" +" {\n" +" gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0);\n" +" gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0);\n" +" \n" +" }\n" +" if (gBodies[bIdx].m_invMass)\n" +" {\n" +" gBodies[bIdx].m_linVel = linVelB;\n" +" gBodies[bIdx].m_angVel = angVelB;\n" +" } else\n" +" {\n" +" gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0);\n" +" gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0);\n" +" \n" +" }\n" +"}\n" +"typedef struct \n" +"{\n" +" int m_valInt0;\n" +" int m_valInt1;\n" +" int m_valInt2;\n" +" int m_valInt3;\n" +" float m_val0;\n" +" float m_val1;\n" +" float m_val2;\n" +" float m_val3;\n" +"} SolverDebugInfo;\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void BatchSolveKernelContact(__global Body* gBodies,\n" +" __global Shape* gShapes,\n" +" __global Constraint4* gConstraints,\n" +" __global int* gN,\n" +" __global int* gOffsets,\n" +" __global int* batchSizes,\n" +" int maxBatch1,\n" +" int cellBatch,\n" +" int4 nSplit\n" +" )\n" +"{\n" +" //__local int ldsBatchIdx[WG_SIZE+1];\n" +" __local int ldsCurBatch;\n" +" __local int ldsNextBatch;\n" +" __local int ldsStart;\n" +" int lIdx = GET_LOCAL_IDX;\n" +" int wgIdx = GET_GROUP_IDX;\n" +"// int gIdx = GET_GLOBAL_IDX;\n" +"// debugInfo[gIdx].m_valInt0 = gIdx;\n" +" //debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE;\n" +" \n" +" \n" +" int zIdx = (wgIdx/((nSplit.x*nSplit.y)/4))*2+((cellBatch&4)>>2);\n" +" int remain= (wgIdx%((nSplit.x*nSplit.y)/4));\n" +" int yIdx = (remain/(nSplit.x/2))*2 + ((cellBatch&2)>>1);\n" +" int xIdx = (remain%(nSplit.x/2))*2 + (cellBatch&1);\n" +" int cellIdx = xIdx+yIdx*nSplit.x+zIdx*(nSplit.x*nSplit.y);\n" +" //int xIdx = (wgIdx/(nSplit/2))*2 + (bIdx&1);\n" +" //int yIdx = (wgIdx%(nSplit/2))*2 + (bIdx>>1);\n" +" //int cellIdx = xIdx+yIdx*nSplit;\n" +" \n" +" if( gN[cellIdx] == 0 ) \n" +" return;\n" +" int maxBatch = batchSizes[cellIdx];\n" +" \n" +" \n" +" const int start = gOffsets[cellIdx];\n" +" const int end = start + gN[cellIdx];\n" +" \n" +" \n" +" \n" +" if( lIdx == 0 )\n" +" {\n" +" ldsCurBatch = 0;\n" +" ldsNextBatch = 0;\n" +" ldsStart = start;\n" +" }\n" +" GROUP_LDS_BARRIER;\n" +" int idx=ldsStart+lIdx;\n" +" while (ldsCurBatch < maxBatch)\n" +" {\n" +" for(; idx 0.70710678f) { + // choose p in y-z plane + float a = n[0].y*n[0].y + n[0].z*n[0].z; + float k = 1.f/sqrt(a); + p[0].x = 0; + p[0].y = -n[0].z*k; + p[0].z = n[0].y*k; + // set q = n x p + q[0].x = a*k; + q[0].y = -n[0].x*p[0].z; + q[0].z = n[0].x*p[0].y; + } + else { + // choose p in x-y plane + float a = n[0].x*n[0].x + n[0].y*n[0].y; + float k = 1.f/sqrt(a); + p[0].x = -n[0].y*k; + p[0].y = n[0].x*k; + p[0].z = 0; + // set q = n x p + q[0].x = -n[0].z*p[0].y; + q[0].y = n[0].z*p[0].x; + q[0].z = a*k; + } +} + + +void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs); +void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs) +{ + float frictionCoeff = ldsCs[0].m_linear.w; + int aIdx = ldsCs[0].m_bodyA; + int bIdx = ldsCs[0].m_bodyB; + + + float4 posA = gBodies[aIdx].m_pos; + float4 linVelA = gBodies[aIdx].m_linVel; + float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia; + + float4 posB = gBodies[bIdx].m_pos; + float4 linVelB = gBodies[bIdx].m_linVel; + float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia; + + + { + float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; + float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; + + float sum = 0; + for(int j=0; j<4; j++) + { + sum +=ldsCs[0].m_appliedRambdaDt[j]; + } + frictionCoeff = 0.7f; + for(int j=0; j<4; j++) + { + maxRambdaDt[j] = frictionCoeff*sum; + minRambdaDt[j] = -maxRambdaDt[j]; + } + + +// solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA, +// posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt ); + + + { + + __global Constraint4* cs = ldsCs; + + if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return; + const float4 center = cs->m_center; + + float4 n = -cs->m_linear; + + float4 tangent[2]; + btPlaneSpace1(&n,&tangent[0],&tangent[1]); + float4 angular0, angular1, linear; + float4 r0 = center - posA; + float4 r1 = center - posB; + for(int i=0; i<2; i++) + { + setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 ); + float rambdaDt = calcRelVel(linear, -linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB ); + rambdaDt *= cs->m_fJacCoeffInv[i]; + + { + float prevSum = cs->m_fAppliedRambdaDt[i]; + float updated = prevSum; + updated += rambdaDt; + updated = max2( updated, minRambdaDt[i] ); + updated = min2( updated, maxRambdaDt[i] ); + rambdaDt = updated - prevSum; + cs->m_fAppliedRambdaDt[i] = updated; + } + + float4 linImp0 = invMassA*linear*rambdaDt; + float4 linImp1 = invMassB*(-linear)*rambdaDt; + float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt; + float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt; + + linVelA += linImp0; + angVelA += angImp0; + linVelB += linImp1; + angVelB += angImp1; + } + { // angular damping for point constraint + float4 ab = normalize3( posB - posA ); + float4 ac = normalize3( center - posA ); + if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) + { + float angNA = dot3F4( n, angVelA ); + float angNB = dot3F4( n, angVelB ); + + angVelA -= (angNA*0.1f)*n; + angVelB -= (angNB*0.1f)*n; + } + } + } + + + + } + + if (gBodies[aIdx].m_invMass) + { + gBodies[aIdx].m_linVel = linVelA; + gBodies[aIdx].m_angVel = angVelA; + } else + { + gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0); + gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0); + } + if (gBodies[bIdx].m_invMass) + { + gBodies[bIdx].m_linVel = linVelB; + gBodies[bIdx].m_angVel = angVelB; + } else + { + gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0); + gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0); + } + + +} + +typedef struct +{ + int m_valInt0; + int m_valInt1; + int m_valInt2; + int m_valInt3; + + float m_val0; + float m_val1; + float m_val2; + float m_val3; +} SolverDebugInfo; + + + + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void BatchSolveKernelFriction(__global Body* gBodies, + __global Shape* gShapes, + __global Constraint4* gConstraints, + __global int* gN, + __global int* gOffsets, + __global int* batchSizes, + int maxBatch1, + int cellBatch, + int4 nSplit + ) +{ + //__local int ldsBatchIdx[WG_SIZE+1]; + __local int ldsCurBatch; + __local int ldsNextBatch; + __local int ldsStart; + + int lIdx = GET_LOCAL_IDX; + int wgIdx = GET_GROUP_IDX; + +// int gIdx = GET_GLOBAL_IDX; +// debugInfo[gIdx].m_valInt0 = gIdx; + //debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE; + + + int zIdx = (wgIdx/((nSplit.x*nSplit.y)/4))*2+((cellBatch&4)>>2); + int remain= (wgIdx%((nSplit.x*nSplit.y)/4)); + int yIdx = (remain/(nSplit.x/2))*2 + ((cellBatch&2)>>1); + int xIdx = (remain%(nSplit.x/2))*2 + (cellBatch&1); + int cellIdx = xIdx+yIdx*nSplit.x+zIdx*(nSplit.x*nSplit.y); + + + if( gN[cellIdx] == 0 ) + return; + + int maxBatch = batchSizes[cellIdx]; + + const int start = gOffsets[cellIdx]; + const int end = start + gN[cellIdx]; + + + if( lIdx == 0 ) + { + ldsCurBatch = 0; + ldsNextBatch = 0; + ldsStart = start; + } + + + GROUP_LDS_BARRIER; + + int idx=ldsStart+lIdx; + while (ldsCurBatch < maxBatch) + { + for(; idx 0.70710678f) {\n" +" // choose p in y-z plane\n" +" float a = n[0].y*n[0].y + n[0].z*n[0].z;\n" +" float k = 1.f/sqrt(a);\n" +" p[0].x = 0;\n" +" p[0].y = -n[0].z*k;\n" +" p[0].z = n[0].y*k;\n" +" // set q = n x p\n" +" q[0].x = a*k;\n" +" q[0].y = -n[0].x*p[0].z;\n" +" q[0].z = n[0].x*p[0].y;\n" +" }\n" +" else {\n" +" // choose p in x-y plane\n" +" float a = n[0].x*n[0].x + n[0].y*n[0].y;\n" +" float k = 1.f/sqrt(a);\n" +" p[0].x = -n[0].y*k;\n" +" p[0].y = n[0].x*k;\n" +" p[0].z = 0;\n" +" // set q = n x p\n" +" q[0].x = -n[0].z*p[0].y;\n" +" q[0].y = n[0].z*p[0].x;\n" +" q[0].z = a*k;\n" +" }\n" +"}\n" +"void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs);\n" +"void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs)\n" +"{\n" +" float frictionCoeff = ldsCs[0].m_linear.w;\n" +" int aIdx = ldsCs[0].m_bodyA;\n" +" int bIdx = ldsCs[0].m_bodyB;\n" +" float4 posA = gBodies[aIdx].m_pos;\n" +" float4 linVelA = gBodies[aIdx].m_linVel;\n" +" float4 angVelA = gBodies[aIdx].m_angVel;\n" +" float invMassA = gBodies[aIdx].m_invMass;\n" +" Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" +" float4 posB = gBodies[bIdx].m_pos;\n" +" float4 linVelB = gBodies[bIdx].m_linVel;\n" +" float4 angVelB = gBodies[bIdx].m_angVel;\n" +" float invMassB = gBodies[bIdx].m_invMass;\n" +" Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" +" \n" +" {\n" +" float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};\n" +" float minRambdaDt[4] = {0.f,0.f,0.f,0.f};\n" +" float sum = 0;\n" +" for(int j=0; j<4; j++)\n" +" {\n" +" sum +=ldsCs[0].m_appliedRambdaDt[j];\n" +" }\n" +" frictionCoeff = 0.7f;\n" +" for(int j=0; j<4; j++)\n" +" {\n" +" maxRambdaDt[j] = frictionCoeff*sum;\n" +" minRambdaDt[j] = -maxRambdaDt[j];\n" +" }\n" +" \n" +"// solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n" +"// posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt );\n" +" \n" +" \n" +" {\n" +" \n" +" __global Constraint4* cs = ldsCs;\n" +" \n" +" if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return;\n" +" const float4 center = cs->m_center;\n" +" \n" +" float4 n = -cs->m_linear;\n" +" \n" +" float4 tangent[2];\n" +" btPlaneSpace1(&n,&tangent[0],&tangent[1]);\n" +" float4 angular0, angular1, linear;\n" +" float4 r0 = center - posA;\n" +" float4 r1 = center - posB;\n" +" for(int i=0; i<2; i++)\n" +" {\n" +" setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 );\n" +" float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,\n" +" linVelA, angVelA, linVelB, angVelB );\n" +" rambdaDt *= cs->m_fJacCoeffInv[i];\n" +" \n" +" {\n" +" float prevSum = cs->m_fAppliedRambdaDt[i];\n" +" float updated = prevSum;\n" +" updated += rambdaDt;\n" +" updated = max2( updated, minRambdaDt[i] );\n" +" updated = min2( updated, maxRambdaDt[i] );\n" +" rambdaDt = updated - prevSum;\n" +" cs->m_fAppliedRambdaDt[i] = updated;\n" +" }\n" +" \n" +" float4 linImp0 = invMassA*linear*rambdaDt;\n" +" float4 linImp1 = invMassB*(-linear)*rambdaDt;\n" +" float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n" +" float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n" +" \n" +" linVelA += linImp0;\n" +" angVelA += angImp0;\n" +" linVelB += linImp1;\n" +" angVelB += angImp1;\n" +" }\n" +" { // angular damping for point constraint\n" +" float4 ab = normalize3( posB - posA );\n" +" float4 ac = normalize3( center - posA );\n" +" if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))\n" +" {\n" +" float angNA = dot3F4( n, angVelA );\n" +" float angNB = dot3F4( n, angVelB );\n" +" \n" +" angVelA -= (angNA*0.1f)*n;\n" +" angVelB -= (angNB*0.1f)*n;\n" +" }\n" +" }\n" +" }\n" +" \n" +" \n" +" }\n" +" if (gBodies[aIdx].m_invMass)\n" +" {\n" +" gBodies[aIdx].m_linVel = linVelA;\n" +" gBodies[aIdx].m_angVel = angVelA;\n" +" } else\n" +" {\n" +" gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0);\n" +" gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0);\n" +" }\n" +" if (gBodies[bIdx].m_invMass)\n" +" {\n" +" gBodies[bIdx].m_linVel = linVelB;\n" +" gBodies[bIdx].m_angVel = angVelB;\n" +" } else\n" +" {\n" +" gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0);\n" +" gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0);\n" +" }\n" +" \n" +"}\n" +"typedef struct \n" +"{\n" +" int m_valInt0;\n" +" int m_valInt1;\n" +" int m_valInt2;\n" +" int m_valInt3;\n" +" float m_val0;\n" +" float m_val1;\n" +" float m_val2;\n" +" float m_val3;\n" +"} SolverDebugInfo;\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void BatchSolveKernelFriction(__global Body* gBodies,\n" +" __global Shape* gShapes,\n" +" __global Constraint4* gConstraints,\n" +" __global int* gN,\n" +" __global int* gOffsets,\n" +" __global int* batchSizes,\n" +" int maxBatch1,\n" +" int cellBatch,\n" +" int4 nSplit\n" +" )\n" +"{\n" +" //__local int ldsBatchIdx[WG_SIZE+1];\n" +" __local int ldsCurBatch;\n" +" __local int ldsNextBatch;\n" +" __local int ldsStart;\n" +" int lIdx = GET_LOCAL_IDX;\n" +" int wgIdx = GET_GROUP_IDX;\n" +"// int gIdx = GET_GLOBAL_IDX;\n" +"// debugInfo[gIdx].m_valInt0 = gIdx;\n" +" //debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE;\n" +" int zIdx = (wgIdx/((nSplit.x*nSplit.y)/4))*2+((cellBatch&4)>>2);\n" +" int remain= (wgIdx%((nSplit.x*nSplit.y)/4));\n" +" int yIdx = (remain/(nSplit.x/2))*2 + ((cellBatch&2)>>1);\n" +" int xIdx = (remain%(nSplit.x/2))*2 + (cellBatch&1);\n" +" int cellIdx = xIdx+yIdx*nSplit.x+zIdx*(nSplit.x*nSplit.y);\n" +" \n" +" if( gN[cellIdx] == 0 ) \n" +" return;\n" +" int maxBatch = batchSizes[cellIdx];\n" +" const int start = gOffsets[cellIdx];\n" +" const int end = start + gN[cellIdx];\n" +" \n" +" if( lIdx == 0 )\n" +" {\n" +" ldsCurBatch = 0;\n" +" ldsNextBatch = 0;\n" +" ldsStart = start;\n" +" }\n" +" GROUP_LDS_BARRIER;\n" +" int idx=ldsStart+lIdx;\n" +" while (ldsCurBatch < maxBatch)\n" +" {\n" +" for(; idx1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" +" return false;\n" +" return true;\n" +"}\n" +"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" +"{\n" +" float maxDot = -B3_INFINITY;\n" +" int i = 0;\n" +" int ptIndex = -1;\n" +" for( i = 0; i < vecLen; i++ )\n" +" {\n" +" float dot = b3Dot3F4(vecArray[i],vec);\n" +" \n" +" if( dot > maxDot )\n" +" {\n" +" maxDot = dot;\n" +" ptIndex = i;\n" +" }\n" +" }\n" +" b3Assert(ptIndex>=0);\n" +" if (ptIndex<0)\n" +" {\n" +" ptIndex = 0;\n" +" }\n" +" *dotOut = maxDot;\n" +" return ptIndex;\n" +"}\n" +"#endif //B3_FLOAT4_H\n" +"typedef struct b3Contact4Data b3Contact4Data_t;\n" +"struct b3Contact4Data\n" +"{\n" +" b3Float4 m_worldPosB[4];\n" +"// b3Float4 m_localPosA[4];\n" +"// b3Float4 m_localPosB[4];\n" +" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" +" unsigned short m_restituitionCoeffCmp;\n" +" unsigned short m_frictionCoeffCmp;\n" +" int m_batchIdx;\n" +" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" +" int m_bodyBPtrAndSignBit;\n" +" int m_childIndexA;\n" +" int m_childIndexB;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"};\n" +"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" +"{\n" +" return (int)contact->m_worldNormalOnB.w;\n" +"};\n" +"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" +"{\n" +" contact->m_worldNormalOnB.w = (float)numPoints;\n" +"};\n" +"#endif //B3_CONTACT4DATA_H\n" +"#ifndef B3_CONTACT_CONSTRAINT5_H\n" +"#define B3_CONTACT_CONSTRAINT5_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"typedef struct b3ContactConstraint4 b3ContactConstraint4_t;\n" +"struct b3ContactConstraint4\n" +"{\n" +" b3Float4 m_linear;//normal?\n" +" b3Float4 m_worldPos[4];\n" +" b3Float4 m_center; // friction\n" +" float m_jacCoeffInv[4];\n" +" float m_b[4];\n" +" float m_appliedRambdaDt[4];\n" +" float m_fJacCoeffInv[2]; // friction\n" +" float m_fAppliedRambdaDt[2]; // friction\n" +" unsigned int m_bodyA;\n" +" unsigned int m_bodyB;\n" +" int m_batchIdx;\n" +" unsigned int m_paddings;\n" +"};\n" +"//inline void setFrictionCoeff(float value) { m_linear[3] = value; }\n" +"inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint) \n" +"{\n" +" return constraint->m_linear.w; \n" +"}\n" +"#endif //B3_CONTACT_CONSTRAINT5_H\n" +"#ifndef B3_RIGIDBODY_DATA_H\n" +"#define B3_RIGIDBODY_DATA_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_QUAT_H\n" +"#define B3_QUAT_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif\n" +"#endif\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Quat;\n" +" #define b3QuatConstArg const b3Quat\n" +" \n" +" \n" +"inline float4 b3FastNormalize4(float4 v)\n" +"{\n" +" v = (float4)(v.xyz,0.f);\n" +" return fast_normalize(v);\n" +"}\n" +" \n" +"inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" +"inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" +"inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" +"{\n" +" b3Quat ans;\n" +" ans = b3Cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - b3Dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" +"{\n" +" b3Quat q;\n" +" q=in;\n" +" //return b3FastNormalize4(in);\n" +" float len = native_sqrt(dot(q, q));\n" +" if(len > 0.f)\n" +" {\n" +" q *= 1.f / len;\n" +" }\n" +" else\n" +" {\n" +" q.x = q.y = q.z = 0.f;\n" +" q.w = 1.f;\n" +" }\n" +" return q;\n" +"}\n" +"inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" b3Quat qInv = b3QuatInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" return b3QuatRotate( b3QuatInvert( q ), vec );\n" +"}\n" +"inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" +"{\n" +" return b3QuatRotate( orientation, point ) + (translation);\n" +"}\n" +" \n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifndef B3_MAT3x3_H\n" +"#define B3_MAT3x3_H\n" +"#ifndef B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"typedef struct\n" +"{\n" +" b3Float4 m_row[3];\n" +"}b3Mat3x3;\n" +"#define b3Mat3x3ConstArg const b3Mat3x3\n" +"#define b3GetRow(m,row) (m.m_row[row])\n" +"inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" +"{\n" +" b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" +" b3Mat3x3 out;\n" +" out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" +" out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" +" out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" +" out.m_row[0].w = 0.f;\n" +" out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" +" out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" +" out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" +" out.m_row[1].w = 0.f;\n" +" out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" +" out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" +" out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" +" out.m_row[2].w = 0.f;\n" +" return out;\n" +"}\n" +"inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = fabs(matIn.m_row[0]);\n" +" out.m_row[1] = fabs(matIn.m_row[1]);\n" +" out.m_row[2] = fabs(matIn.m_row[2]);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtZero();\n" +"__inline\n" +"b3Mat3x3 mtIdentity();\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Mat3x3 mtZero()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(0.f);\n" +" m.m_row[1] = (b3Float4)(0.f);\n" +" m.m_row[2] = (b3Float4)(0.f);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtIdentity()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(1,0,0,0);\n" +" m.m_row[1] = (b3Float4)(0,1,0,0);\n" +" m.m_row[2] = (b3Float4)(0,0,1,0);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" +" out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" +" out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" +"{\n" +" b3Mat3x3 transB;\n" +" transB = mtTranspose( b );\n" +" b3Mat3x3 ans;\n" +" // why this doesn't run when 0ing in the for{}\n" +" a.m_row[0].w = 0.f;\n" +" a.m_row[1].w = 0.f;\n" +" a.m_row[2].w = 0.f;\n" +" for(int i=0; i<3; i++)\n" +" {\n" +"// a.m_row[i].w = 0.f;\n" +" ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" +" ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" +" ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" +" ans.m_row[i].w = 0.f;\n" +" }\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" +"{\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a.m_row[0], b );\n" +" ans.y = b3Dot3F4( a.m_row[1], b );\n" +" ans.z = b3Dot3F4( a.m_row[2], b );\n" +" ans.w = 0.f;\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" +"{\n" +" b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" +" b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" +" b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a, colx );\n" +" ans.y = b3Dot3F4( a, coly );\n" +" ans.z = b3Dot3F4( a, colz );\n" +" return ans;\n" +"}\n" +"#endif\n" +"#endif //B3_MAT3x3_H\n" +"typedef struct b3RigidBodyData b3RigidBodyData_t;\n" +"struct b3RigidBodyData\n" +"{\n" +" b3Float4 m_pos;\n" +" b3Quat m_quat;\n" +" b3Float4 m_linVel;\n" +" b3Float4 m_angVel;\n" +" int m_collidableIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"};\n" +"typedef struct b3InertiaData b3InertiaData_t;\n" +"struct b3InertiaData\n" +"{\n" +" b3Mat3x3 m_invInertiaWorld;\n" +" b3Mat3x3 m_initInvInertia;\n" +"};\n" +"#endif //B3_RIGIDBODY_DATA_H\n" +" \n" +"void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q);\n" +" void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q)\n" +"{\n" +" if (b3Fabs(n.z) > 0.70710678f) {\n" +" // choose p in y-z plane\n" +" float a = n.y*n.y + n.z*n.z;\n" +" float k = 1.f/sqrt(a);\n" +" p[0].x = 0;\n" +" p[0].y = -n.z*k;\n" +" p[0].z = n.y*k;\n" +" // set q = n x p\n" +" q[0].x = a*k;\n" +" q[0].y = -n.x*p[0].z;\n" +" q[0].z = n.x*p[0].y;\n" +" }\n" +" else {\n" +" // choose p in x-y plane\n" +" float a = n.x*n.x + n.y*n.y;\n" +" float k = 1.f/sqrt(a);\n" +" p[0].x = -n.y*k;\n" +" p[0].y = n.x*k;\n" +" p[0].z = 0;\n" +" // set q = n x p\n" +" q[0].x = -n.z*p[0].y;\n" +" q[0].y = n.z*p[0].x;\n" +" q[0].z = a*k;\n" +" }\n" +"}\n" +" \n" +"void setLinearAndAngular( b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)\n" +"{\n" +" *linear = b3MakeFloat4(n.x,n.y,n.z,0.f);\n" +" *angular0 = b3Cross3(r0, n);\n" +" *angular1 = -b3Cross3(r1, n);\n" +"}\n" +"float calcRelVel( b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,\n" +" b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1 )\n" +"{\n" +" return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1);\n" +"}\n" +"float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1,\n" +" float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)\n" +"{\n" +" // linear0,1 are normlized\n" +" float jmj0 = invMass0;//b3Dot3F4(linear0, linear0)*invMass0;\n" +" float jmj1 = b3Dot3F4(mtMul3(angular0,*invInertia0), angular0);\n" +" float jmj2 = invMass1;//b3Dot3F4(linear1, linear1)*invMass1;\n" +" float jmj3 = b3Dot3F4(mtMul3(angular1,*invInertia1), angular1);\n" +" return -1.f/(jmj0+jmj1+jmj2+jmj3);\n" +"}\n" +"void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,\n" +" b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB, \n" +" __global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,\n" +" b3ContactConstraint4_t* dstC )\n" +"{\n" +" dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);\n" +" dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);\n" +" float dtInv = 1.f/dt;\n" +" for(int ic=0; ic<4; ic++)\n" +" {\n" +" dstC->m_appliedRambdaDt[ic] = 0.f;\n" +" }\n" +" dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;\n" +" dstC->m_linear = src->m_worldNormalOnB;\n" +" dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );\n" +" for(int ic=0; ic<4; ic++)\n" +" {\n" +" b3Float4 r0 = src->m_worldPosB[ic] - posA;\n" +" b3Float4 r1 = src->m_worldPosB[ic] - posB;\n" +" if( ic >= src->m_worldNormalOnB.w )//npoints\n" +" {\n" +" dstC->m_jacCoeffInv[ic] = 0.f;\n" +" continue;\n" +" }\n" +" float relVelN;\n" +" {\n" +" b3Float4 linear, angular0, angular1;\n" +" setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);\n" +" dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,\n" +" invMassA, &invInertiaA, invMassB, &invInertiaB );\n" +" relVelN = calcRelVel(linear, -linear, angular0, angular1,\n" +" linVelA, angVelA, linVelB, angVelB);\n" +" float e = 0.f;//src->getRestituitionCoeff();\n" +" if( relVelN*relVelN < 0.004f ) e = 0.f;\n" +" dstC->m_b[ic] = e*relVelN;\n" +" //float penetration = src->m_worldPosB[ic].w;\n" +" dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;\n" +" dstC->m_appliedRambdaDt[ic] = 0.f;\n" +" }\n" +" }\n" +" if( src->m_worldNormalOnB.w > 0 )//npoints\n" +" { // prepare friction\n" +" b3Float4 center = b3MakeFloat4(0.f,0.f,0.f,0.f);\n" +" for(int i=0; im_worldNormalOnB.w; i++) \n" +" center += src->m_worldPosB[i];\n" +" center /= (float)src->m_worldNormalOnB.w;\n" +" b3Float4 tangent[2];\n" +" b3PlaneSpace1(src->m_worldNormalOnB,&tangent[0],&tangent[1]);\n" +" \n" +" b3Float4 r[2];\n" +" r[0] = center - posA;\n" +" r[1] = center - posB;\n" +" for(int i=0; i<2; i++)\n" +" {\n" +" b3Float4 linear, angular0, angular1;\n" +" setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);\n" +" dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,\n" +" invMassA, &invInertiaA, invMassB, &invInertiaB );\n" +" dstC->m_fAppliedRambdaDt[i] = 0.f;\n" +" }\n" +" dstC->m_center = center;\n" +" }\n" +" for(int i=0; i<4; i++)\n" +" {\n" +" if( im_worldNormalOnB.w )\n" +" {\n" +" dstC->m_worldPos[i] = src->m_worldPosB[i];\n" +" }\n" +" else\n" +" {\n" +" dstC->m_worldPos[i] = b3MakeFloat4(0.f,0.f,0.f,0.f);\n" +" }\n" +" }\n" +"}\n" +"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" +"#ifdef cl_ext_atomic_counters_32\n" +"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" +"#else\n" +"#define counter32_t volatile global int*\n" +"#endif\n" +"typedef unsigned int u32;\n" +"typedef unsigned short u16;\n" +"typedef unsigned char u8;\n" +"#define GET_GROUP_IDX get_group_id(0)\n" +"#define GET_LOCAL_IDX get_local_id(0)\n" +"#define GET_GLOBAL_IDX get_global_id(0)\n" +"#define GET_GROUP_SIZE get_local_size(0)\n" +"#define GET_NUM_GROUPS get_num_groups(0)\n" +"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" +"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" +"#define AtomInc(x) atom_inc(&(x))\n" +"#define AtomInc1(x, out) out = atom_inc(&(x))\n" +"#define AppendInc(x, out) out = atomic_inc(x)\n" +"#define AtomAdd(x, value) atom_add(&(x), value)\n" +"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" +"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" +"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" +"#define make_float4 (float4)\n" +"#define make_float2 (float2)\n" +"#define make_uint4 (uint4)\n" +"#define make_int4 (int4)\n" +"#define make_uint2 (uint2)\n" +"#define make_int2 (int2)\n" +"#define max2 max\n" +"#define min2 min\n" +"///////////////////////////////////////\n" +"// Vector\n" +"///////////////////////////////////////\n" +"__inline\n" +"float fastDiv(float numerator, float denominator)\n" +"{\n" +" return native_divide(numerator, denominator); \n" +"// return numerator/denominator; \n" +"}\n" +"__inline\n" +"float4 fastDiv4(float4 numerator, float4 denominator)\n" +"{\n" +" return native_divide(numerator, denominator); \n" +"}\n" +"__inline\n" +"float fastSqrtf(float f2)\n" +"{\n" +" return native_sqrt(f2);\n" +"// return sqrt(f2);\n" +"}\n" +"__inline\n" +"float fastRSqrt(float f2)\n" +"{\n" +" return native_rsqrt(f2);\n" +"}\n" +"__inline\n" +"float fastLength4(float4 v)\n" +"{\n" +" return fast_length(v);\n" +"}\n" +"__inline\n" +"float4 fastNormalize4(float4 v)\n" +"{\n" +" return fast_normalize(v);\n" +"}\n" +"__inline\n" +"float sqrtf(float a)\n" +"{\n" +"// return sqrt(a);\n" +" return native_sqrt(a);\n" +"}\n" +"__inline\n" +"float4 cross3(float4 a, float4 b)\n" +"{\n" +" return cross(a,b);\n" +"}\n" +"__inline\n" +"float dot3F4(float4 a, float4 b)\n" +"{\n" +" float4 a1 = make_float4(a.xyz,0.f);\n" +" float4 b1 = make_float4(b.xyz,0.f);\n" +" return dot(a1, b1);\n" +"}\n" +"__inline\n" +"float length3(const float4 a)\n" +"{\n" +" return sqrtf(dot3F4(a,a));\n" +"}\n" +"__inline\n" +"float dot4(const float4 a, const float4 b)\n" +"{\n" +" return dot( a, b );\n" +"}\n" +"// for height\n" +"__inline\n" +"float dot3w1(const float4 point, const float4 eqn)\n" +"{\n" +" return dot3F4(point,eqn) + eqn.w;\n" +"}\n" +"__inline\n" +"float4 normalize3(const float4 a)\n" +"{\n" +" float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" +" return fastNormalize4( n );\n" +"// float length = sqrtf(dot3F4(a, a));\n" +"// return 1.f/length * a;\n" +"}\n" +"__inline\n" +"float4 normalize4(const float4 a)\n" +"{\n" +" float length = sqrtf(dot4(a, a));\n" +" return 1.f/length * a;\n" +"}\n" +"__inline\n" +"float4 createEquation(const float4 a, const float4 b, const float4 c)\n" +"{\n" +" float4 eqn;\n" +" float4 ab = b-a;\n" +" float4 ac = c-a;\n" +" eqn = normalize3( cross3(ab, ac) );\n" +" eqn.w = -dot3F4(eqn,a);\n" +" return eqn;\n" +"}\n" +"#define WG_SIZE 64\n" +"typedef struct\n" +"{\n" +" int m_nConstraints;\n" +" int m_start;\n" +" int m_batchIdx;\n" +" int m_nSplit;\n" +"// int m_paddings[1];\n" +"} ConstBuffer;\n" +"typedef struct\n" +"{\n" +" int m_solveFriction;\n" +" int m_maxBatch; // long batch really kills the performance\n" +" int m_batchIdx;\n" +" int m_nSplit;\n" +"// int m_paddings[1];\n" +"} ConstBufferBatchSolve;\n" +" \n" +"typedef struct \n" +"{\n" +" int m_valInt0;\n" +" int m_valInt1;\n" +" int m_valInt2;\n" +" int m_valInt3;\n" +" float m_val0;\n" +" float m_val1;\n" +" float m_val2;\n" +" float m_val3;\n" +"} SolverDebugInfo;\n" +"typedef struct\n" +"{\n" +" int m_nContacts;\n" +" float m_dt;\n" +" float m_positionDrift;\n" +" float m_positionConstraintCoeff;\n" +"} ConstBufferCTC;\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void ContactToConstraintKernel(__global struct b3Contact4Data* gContact, __global b3RigidBodyData_t* gBodies, __global b3InertiaData_t* gShapes, __global b3ContactConstraint4_t* gConstraintOut, \n" +"int nContacts,\n" +"float dt,\n" +"float positionDrift,\n" +"float positionConstraintCoeff\n" +")\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" \n" +" if( gIdx < nContacts )\n" +" {\n" +" int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);\n" +" int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);\n" +" float4 posA = gBodies[aIdx].m_pos;\n" +" float4 linVelA = gBodies[aIdx].m_linVel;\n" +" float4 angVelA = gBodies[aIdx].m_angVel;\n" +" float invMassA = gBodies[aIdx].m_invMass;\n" +" b3Mat3x3 invInertiaA = gShapes[aIdx].m_initInvInertia;\n" +" float4 posB = gBodies[bIdx].m_pos;\n" +" float4 linVelB = gBodies[bIdx].m_linVel;\n" +" float4 angVelB = gBodies[bIdx].m_angVel;\n" +" float invMassB = gBodies[bIdx].m_invMass;\n" +" b3Mat3x3 invInertiaB = gShapes[bIdx].m_initInvInertia;\n" +" b3ContactConstraint4_t cs;\n" +" setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,\n" +" &gContact[gIdx], dt, positionDrift, positionConstraintCoeff,\n" +" &cs );\n" +" \n" +" cs.m_batchIdx = gContact[gIdx].m_batchIdx;\n" +" gConstraintOut[gIdx] = cs;\n" +" }\n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl new file mode 100644 index 000000000000..3dc48d435021 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl @@ -0,0 +1,613 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, 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. +*/ +//Originally written by Takahiro Harada + + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable + + +#ifdef cl_ext_atomic_counters_32 +#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable +#else +#define counter32_t volatile global int* +#endif + +typedef unsigned int u32; +typedef unsigned short u16; +typedef unsigned char u8; + +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GET_NUM_GROUPS get_num_groups(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) +#define AppendInc(x, out) out = atomic_inc(x) +#define AtomAdd(x, value) atom_add(&(x), value) +#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value ) +#define AtomXhg(x, value) atom_xchg ( &(x), value ) + + +#define SELECT_UINT4( b, a, condition ) select( b,a,condition ) + +#define make_float4 (float4) +#define make_float2 (float2) +#define make_uint4 (uint4) +#define make_int4 (int4) +#define make_uint2 (uint2) +#define make_int2 (int2) + + +#define max2 max +#define min2 min + + +/////////////////////////////////////// +// Vector +/////////////////////////////////////// +__inline +float fastDiv(float numerator, float denominator) +{ + return native_divide(numerator, denominator); +// return numerator/denominator; +} + +__inline +float4 fastDiv4(float4 numerator, float4 denominator) +{ + return native_divide(numerator, denominator); +} + +__inline +float fastSqrtf(float f2) +{ + return native_sqrt(f2); +// return sqrt(f2); +} + +__inline +float fastRSqrt(float f2) +{ + return native_rsqrt(f2); +} + +__inline +float fastLength4(float4 v) +{ + return fast_length(v); +} + +__inline +float4 fastNormalize4(float4 v) +{ + return fast_normalize(v); +} + + +__inline +float sqrtf(float a) +{ +// return sqrt(a); + return native_sqrt(a); +} + +__inline +float4 cross3(float4 a, float4 b) +{ + return cross(a,b); +} + +__inline +float dot3F4(float4 a, float4 b) +{ + float4 a1 = make_float4(a.xyz,0.f); + float4 b1 = make_float4(b.xyz,0.f); + return dot(a1, b1); +} + +__inline +float length3(const float4 a) +{ + return sqrtf(dot3F4(a,a)); +} + +__inline +float dot4(const float4 a, const float4 b) +{ + return dot( a, b ); +} + +// for height +__inline +float dot3w1(const float4 point, const float4 eqn) +{ + return dot3F4(point,eqn) + eqn.w; +} + +__inline +float4 normalize3(const float4 a) +{ + float4 n = make_float4(a.x, a.y, a.z, 0.f); + return fastNormalize4( n ); +// float length = sqrtf(dot3F4(a, a)); +// return 1.f/length * a; +} + +__inline +float4 normalize4(const float4 a) +{ + float length = sqrtf(dot4(a, a)); + return 1.f/length * a; +} + +__inline +float4 createEquation(const float4 a, const float4 b, const float4 c) +{ + float4 eqn; + float4 ab = b-a; + float4 ac = c-a; + eqn = normalize3( cross3(ab, ac) ); + eqn.w = -dot3F4(eqn,a); + return eqn; +} + +/////////////////////////////////////// +// Matrix3x3 +/////////////////////////////////////// + +typedef struct +{ + float4 m_row[3]; +}Matrix3x3; + +__inline +Matrix3x3 mtZero(); + +__inline +Matrix3x3 mtIdentity(); + +__inline +Matrix3x3 mtTranspose(Matrix3x3 m); + +__inline +Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b); + +__inline +float4 mtMul1(Matrix3x3 a, float4 b); + +__inline +float4 mtMul3(float4 a, Matrix3x3 b); + +__inline +Matrix3x3 mtZero() +{ + Matrix3x3 m; + m.m_row[0] = (float4)(0.f); + m.m_row[1] = (float4)(0.f); + m.m_row[2] = (float4)(0.f); + return m; +} + +__inline +Matrix3x3 mtIdentity() +{ + Matrix3x3 m; + m.m_row[0] = (float4)(1,0,0,0); + m.m_row[1] = (float4)(0,1,0,0); + m.m_row[2] = (float4)(0,0,1,0); + return m; +} + +__inline +Matrix3x3 mtTranspose(Matrix3x3 m) +{ + Matrix3x3 out; + out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f); + out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f); + out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f); + return out; +} + +__inline +Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b) +{ + Matrix3x3 transB; + transB = mtTranspose( b ); + Matrix3x3 ans; + // why this doesn't run when 0ing in the for{} + a.m_row[0].w = 0.f; + a.m_row[1].w = 0.f; + a.m_row[2].w = 0.f; + for(int i=0; i<3; i++) + { +// a.m_row[i].w = 0.f; + ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]); + ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]); + ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]); + ans.m_row[i].w = 0.f; + } + return ans; +} + +__inline +float4 mtMul1(Matrix3x3 a, float4 b) +{ + float4 ans; + ans.x = dot3F4( a.m_row[0], b ); + ans.y = dot3F4( a.m_row[1], b ); + ans.z = dot3F4( a.m_row[2], b ); + ans.w = 0.f; + return ans; +} + +__inline +float4 mtMul3(float4 a, Matrix3x3 b) +{ + float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0); + float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0); + float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0); + + float4 ans; + ans.x = dot3F4( a, colx ); + ans.y = dot3F4( a, coly ); + ans.z = dot3F4( a, colz ); + return ans; +} + +/////////////////////////////////////// +// Quaternion +/////////////////////////////////////// + +typedef float4 Quaternion; + +__inline +Quaternion qtMul(Quaternion a, Quaternion b); + +__inline +Quaternion qtNormalize(Quaternion in); + +__inline +float4 qtRotate(Quaternion q, float4 vec); + +__inline +Quaternion qtInvert(Quaternion q); + + + + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b) +{ + Quaternion ans; + ans = cross3( a, b ); + ans += a.w*b+b.w*a; +// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - dot3F4(a, b); + return ans; +} + +__inline +Quaternion qtNormalize(Quaternion in) +{ + return fastNormalize4(in); +// in /= length( in ); +// return in; +} +__inline +float4 qtRotate(Quaternion q, float4 vec) +{ + Quaternion qInv = qtInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = qtMul(qtMul(q,vcpy),qInv); + return out; +} + +__inline +Quaternion qtInvert(Quaternion q) +{ + return (Quaternion)(-q.xyz, q.w); +} + +__inline +float4 qtInvRotate(const Quaternion q, float4 vec) +{ + return qtRotate( qtInvert( q ), vec ); +} + + + + +#define WG_SIZE 64 + +typedef struct +{ + float4 m_pos; + Quaternion m_quat; + float4 m_linVel; + float4 m_angVel; + + u32 m_shapeIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} Body; + +typedef struct +{ + Matrix3x3 m_invInertia; + Matrix3x3 m_initInvInertia; +} Shape; + +typedef struct +{ + float4 m_linear; + float4 m_worldPos[4]; + float4 m_center; + float m_jacCoeffInv[4]; + float m_b[4]; + float m_appliedRambdaDt[4]; + + float m_fJacCoeffInv[2]; + float m_fAppliedRambdaDt[2]; + + u32 m_bodyA; + u32 m_bodyB; + + int m_batchIdx; + u32 m_paddings[1]; +} Constraint4; + + + +typedef struct +{ + int m_nConstraints; + int m_start; + int m_batchIdx; + int m_nSplit; +// int m_paddings[1]; +} ConstBuffer; + +typedef struct +{ + int m_solveFriction; + int m_maxBatch; // long batch really kills the performance + int m_batchIdx; + int m_nSplit; +// int m_paddings[1]; +} ConstBufferBatchSolve; + + + + + +typedef struct +{ + int m_valInt0; + int m_valInt1; + int m_valInt2; + int m_valInt3; + + float m_val0; + float m_val1; + float m_val2; + float m_val3; +} SolverDebugInfo; + + + + +// others +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void ReorderContactKernel(__global struct b3Contact4Data* in, __global struct b3Contact4Data* out, __global int2* sortData, int4 cb ) +{ + int nContacts = cb.x; + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int srcIdx = sortData[gIdx].y; + out[gIdx] = in[srcIdx]; + } +} + +__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void SetDeterminismSortDataChildShapeB(__global struct b3Contact4Data* contactsIn, __global int2* sortDataOut, int nContacts) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int2 sd; + sd.x = contactsIn[gIdx].m_childIndexB; + sd.y = gIdx; + sortDataOut[gIdx] = sd; + } +} + +__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void SetDeterminismSortDataChildShapeA(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int2 sdIn; + sdIn = sortDataInOut[gIdx]; + int2 sdOut; + sdOut.x = contactsIn[sdIn.y].m_childIndexA; + sdOut.y = sdIn.y; + sortDataInOut[gIdx] = sdOut; + } +} + +__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void SetDeterminismSortDataBodyA(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int2 sdIn; + sdIn = sortDataInOut[gIdx]; + int2 sdOut; + sdOut.x = contactsIn[sdIn.y].m_bodyAPtrAndSignBit; + sdOut.y = sdIn.y; + sortDataInOut[gIdx] = sdOut; + } +} + + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void SetDeterminismSortDataBodyB(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int2 sdIn; + sdIn = sortDataInOut[gIdx]; + int2 sdOut; + sdOut.x = contactsIn[sdIn.y].m_bodyBPtrAndSignBit; + sdOut.y = sdIn.y; + sortDataInOut[gIdx] = sdOut; + } +} + + + + +typedef struct +{ + int m_nContacts; + int m_staticIdx; + float m_scale; + int m_nSplit; +} ConstBufferSSD; + + +__constant const int gridTable4x4[] = +{ + 0,1,17,16, + 1,2,18,19, + 17,18,32,3, + 16,19,3,34 +}; + +__constant const int gridTable8x8[] = +{ + 0, 2, 3, 16, 17, 18, 19, 1, + 66, 64, 80, 67, 82, 81, 65, 83, + 131,144,128,130,147,129,145,146, + 208,195,194,192,193,211,210,209, + 21, 22, 23, 5, 4, 6, 7, 20, + 86, 85, 69, 87, 70, 68, 84, 71, + 151,133,149,150,135,148,132,134, + 197,27,214,213,212,199,198,196 + +}; + + + + +#define USE_SPATIAL_BATCHING 1 +#define USE_4x4_GRID 1 + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void SetSortDataKernel(__global struct b3Contact4Data* gContact, __global Body* gBodies, __global int2* gSortDataOut, +int nContacts,float scale,int4 nSplit,int staticIdx) + +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int aPtrAndSignBit = gContact[gIdx].m_bodyAPtrAndSignBit; + int bPtrAndSignBit = gContact[gIdx].m_bodyBPtrAndSignBit; + + int aIdx = abs(aPtrAndSignBit ); + int bIdx = abs(bPtrAndSignBit); + + bool aStatic = (aPtrAndSignBit<0) ||(aPtrAndSignBit==staticIdx); + bool bStatic = (bPtrAndSignBit<0) ||(bPtrAndSignBit==staticIdx); + +#if USE_SPATIAL_BATCHING + int idx = (aStatic)? bIdx: aIdx; + float4 p = gBodies[idx].m_pos; + int xIdx = (int)((p.x-((p.x<0.f)?1.f:0.f))*scale) & (nSplit.x-1); + int yIdx = (int)((p.y-((p.y<0.f)?1.f:0.f))*scale) & (nSplit.y-1); + int zIdx = (int)((p.z-((p.z<0.f)?1.f:0.f))*scale) & (nSplit.z-1); + int newIndex = (xIdx+yIdx*nSplit.x+zIdx*nSplit.x*nSplit.y); + +#else//USE_SPATIAL_BATCHING + #if USE_4x4_GRID + int aa = aIdx&3; + int bb = bIdx&3; + if (aStatic) + aa = bb; + if (bStatic) + bb = aa; + + int gridIndex = aa + bb*4; + int newIndex = gridTable4x4[gridIndex]; + #else//USE_4x4_GRID + int aa = aIdx&7; + int bb = bIdx&7; + if (aStatic) + aa = bb; + if (bStatic) + bb = aa; + + int gridIndex = aa + bb*8; + int newIndex = gridTable8x8[gridIndex]; + #endif//USE_4x4_GRID +#endif//USE_SPATIAL_BATCHING + + + gSortDataOut[gIdx].x = newIndex; + gSortDataOut[gIdx].y = gIdx; + } + else + { + gSortDataOut[gIdx].x = 0xffffffff; + } +} + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void CopyConstraintKernel(__global struct b3Contact4Data* gIn, __global struct b3Contact4Data* gOut, int4 cb ) +{ + int gIdx = GET_GLOBAL_IDX; + if( gIdx < cb.x ) + { + gOut[gIdx] = gIn[gIdx]; + } +} + + + diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h new file mode 100644 index 000000000000..1b5819f6cf32 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h @@ -0,0 +1,601 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* solverSetup2CL= \ +"/*\n" +"Copyright (c) 2012 Advanced Micro Devices, Inc. \n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose, \n" +"including commercial applications, and to alter it and redistribute it freely, \n" +"subject to the following restrictions:\n" +"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.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"//Originally written by Takahiro Harada\n" +"#ifndef B3_CONTACT4DATA_H\n" +"#define B3_CONTACT4DATA_H\n" +"#ifndef B3_FLOAT4_H\n" +"#define B3_FLOAT4_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#define B3_PLATFORM_DEFINITIONS_H\n" +"struct MyTest\n" +"{\n" +" int bla;\n" +"};\n" +"#ifdef __cplusplus\n" +"#else\n" +"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" +"#define B3_LARGE_FLOAT 1e18f\n" +"#define B3_INFINITY 1e18f\n" +"#define b3Assert(a)\n" +"#define b3ConstArray(a) __global const a*\n" +"#define b3AtomicInc atomic_inc\n" +"#define b3AtomicAdd atomic_add\n" +"#define b3Fabs fabs\n" +"#define b3Sqrt native_sqrt\n" +"#define b3Sin native_sin\n" +"#define b3Cos native_cos\n" +"#define B3_STATIC\n" +"#endif\n" +"#endif\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Float4;\n" +" #define b3Float4ConstArg const b3Float4\n" +" #define b3MakeFloat4 (float4)\n" +" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return dot(a1, b1);\n" +" }\n" +" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return cross(a1, b1);\n" +" }\n" +" #define b3MinFloat4 min\n" +" #define b3MaxFloat4 max\n" +" #define b3Normalized(a) normalize(a)\n" +"#endif \n" +" \n" +"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" +"{\n" +" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" +" return false;\n" +" return true;\n" +"}\n" +"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" +"{\n" +" float maxDot = -B3_INFINITY;\n" +" int i = 0;\n" +" int ptIndex = -1;\n" +" for( i = 0; i < vecLen; i++ )\n" +" {\n" +" float dot = b3Dot3F4(vecArray[i],vec);\n" +" \n" +" if( dot > maxDot )\n" +" {\n" +" maxDot = dot;\n" +" ptIndex = i;\n" +" }\n" +" }\n" +" b3Assert(ptIndex>=0);\n" +" if (ptIndex<0)\n" +" {\n" +" ptIndex = 0;\n" +" }\n" +" *dotOut = maxDot;\n" +" return ptIndex;\n" +"}\n" +"#endif //B3_FLOAT4_H\n" +"typedef struct b3Contact4Data b3Contact4Data_t;\n" +"struct b3Contact4Data\n" +"{\n" +" b3Float4 m_worldPosB[4];\n" +"// b3Float4 m_localPosA[4];\n" +"// b3Float4 m_localPosB[4];\n" +" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" +" unsigned short m_restituitionCoeffCmp;\n" +" unsigned short m_frictionCoeffCmp;\n" +" int m_batchIdx;\n" +" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" +" int m_bodyBPtrAndSignBit;\n" +" int m_childIndexA;\n" +" int m_childIndexB;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"};\n" +"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" +"{\n" +" return (int)contact->m_worldNormalOnB.w;\n" +"};\n" +"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" +"{\n" +" contact->m_worldNormalOnB.w = (float)numPoints;\n" +"};\n" +"#endif //B3_CONTACT4DATA_H\n" +"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" +"#ifdef cl_ext_atomic_counters_32\n" +"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" +"#else\n" +"#define counter32_t volatile global int*\n" +"#endif\n" +"typedef unsigned int u32;\n" +"typedef unsigned short u16;\n" +"typedef unsigned char u8;\n" +"#define GET_GROUP_IDX get_group_id(0)\n" +"#define GET_LOCAL_IDX get_local_id(0)\n" +"#define GET_GLOBAL_IDX get_global_id(0)\n" +"#define GET_GROUP_SIZE get_local_size(0)\n" +"#define GET_NUM_GROUPS get_num_groups(0)\n" +"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" +"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" +"#define AtomInc(x) atom_inc(&(x))\n" +"#define AtomInc1(x, out) out = atom_inc(&(x))\n" +"#define AppendInc(x, out) out = atomic_inc(x)\n" +"#define AtomAdd(x, value) atom_add(&(x), value)\n" +"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" +"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" +"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" +"#define make_float4 (float4)\n" +"#define make_float2 (float2)\n" +"#define make_uint4 (uint4)\n" +"#define make_int4 (int4)\n" +"#define make_uint2 (uint2)\n" +"#define make_int2 (int2)\n" +"#define max2 max\n" +"#define min2 min\n" +"///////////////////////////////////////\n" +"// Vector\n" +"///////////////////////////////////////\n" +"__inline\n" +"float fastDiv(float numerator, float denominator)\n" +"{\n" +" return native_divide(numerator, denominator); \n" +"// return numerator/denominator; \n" +"}\n" +"__inline\n" +"float4 fastDiv4(float4 numerator, float4 denominator)\n" +"{\n" +" return native_divide(numerator, denominator); \n" +"}\n" +"__inline\n" +"float fastSqrtf(float f2)\n" +"{\n" +" return native_sqrt(f2);\n" +"// return sqrt(f2);\n" +"}\n" +"__inline\n" +"float fastRSqrt(float f2)\n" +"{\n" +" return native_rsqrt(f2);\n" +"}\n" +"__inline\n" +"float fastLength4(float4 v)\n" +"{\n" +" return fast_length(v);\n" +"}\n" +"__inline\n" +"float4 fastNormalize4(float4 v)\n" +"{\n" +" return fast_normalize(v);\n" +"}\n" +"__inline\n" +"float sqrtf(float a)\n" +"{\n" +"// return sqrt(a);\n" +" return native_sqrt(a);\n" +"}\n" +"__inline\n" +"float4 cross3(float4 a, float4 b)\n" +"{\n" +" return cross(a,b);\n" +"}\n" +"__inline\n" +"float dot3F4(float4 a, float4 b)\n" +"{\n" +" float4 a1 = make_float4(a.xyz,0.f);\n" +" float4 b1 = make_float4(b.xyz,0.f);\n" +" return dot(a1, b1);\n" +"}\n" +"__inline\n" +"float length3(const float4 a)\n" +"{\n" +" return sqrtf(dot3F4(a,a));\n" +"}\n" +"__inline\n" +"float dot4(const float4 a, const float4 b)\n" +"{\n" +" return dot( a, b );\n" +"}\n" +"// for height\n" +"__inline\n" +"float dot3w1(const float4 point, const float4 eqn)\n" +"{\n" +" return dot3F4(point,eqn) + eqn.w;\n" +"}\n" +"__inline\n" +"float4 normalize3(const float4 a)\n" +"{\n" +" float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" +" return fastNormalize4( n );\n" +"// float length = sqrtf(dot3F4(a, a));\n" +"// return 1.f/length * a;\n" +"}\n" +"__inline\n" +"float4 normalize4(const float4 a)\n" +"{\n" +" float length = sqrtf(dot4(a, a));\n" +" return 1.f/length * a;\n" +"}\n" +"__inline\n" +"float4 createEquation(const float4 a, const float4 b, const float4 c)\n" +"{\n" +" float4 eqn;\n" +" float4 ab = b-a;\n" +" float4 ac = c-a;\n" +" eqn = normalize3( cross3(ab, ac) );\n" +" eqn.w = -dot3F4(eqn,a);\n" +" return eqn;\n" +"}\n" +"///////////////////////////////////////\n" +"// Matrix3x3\n" +"///////////////////////////////////////\n" +"typedef struct\n" +"{\n" +" float4 m_row[3];\n" +"}Matrix3x3;\n" +"__inline\n" +"Matrix3x3 mtZero();\n" +"__inline\n" +"Matrix3x3 mtIdentity();\n" +"__inline\n" +"Matrix3x3 mtTranspose(Matrix3x3 m);\n" +"__inline\n" +"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b);\n" +"__inline\n" +"float4 mtMul1(Matrix3x3 a, float4 b);\n" +"__inline\n" +"float4 mtMul3(float4 a, Matrix3x3 b);\n" +"__inline\n" +"Matrix3x3 mtZero()\n" +"{\n" +" Matrix3x3 m;\n" +" m.m_row[0] = (float4)(0.f);\n" +" m.m_row[1] = (float4)(0.f);\n" +" m.m_row[2] = (float4)(0.f);\n" +" return m;\n" +"}\n" +"__inline\n" +"Matrix3x3 mtIdentity()\n" +"{\n" +" Matrix3x3 m;\n" +" m.m_row[0] = (float4)(1,0,0,0);\n" +" m.m_row[1] = (float4)(0,1,0,0);\n" +" m.m_row[2] = (float4)(0,0,1,0);\n" +" return m;\n" +"}\n" +"__inline\n" +"Matrix3x3 mtTranspose(Matrix3x3 m)\n" +"{\n" +" Matrix3x3 out;\n" +" out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" +" out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" +" out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" +" return out;\n" +"}\n" +"__inline\n" +"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)\n" +"{\n" +" Matrix3x3 transB;\n" +" transB = mtTranspose( b );\n" +" Matrix3x3 ans;\n" +" // why this doesn't run when 0ing in the for{}\n" +" a.m_row[0].w = 0.f;\n" +" a.m_row[1].w = 0.f;\n" +" a.m_row[2].w = 0.f;\n" +" for(int i=0; i<3; i++)\n" +" {\n" +"// a.m_row[i].w = 0.f;\n" +" ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);\n" +" ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);\n" +" ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);\n" +" ans.m_row[i].w = 0.f;\n" +" }\n" +" return ans;\n" +"}\n" +"__inline\n" +"float4 mtMul1(Matrix3x3 a, float4 b)\n" +"{\n" +" float4 ans;\n" +" ans.x = dot3F4( a.m_row[0], b );\n" +" ans.y = dot3F4( a.m_row[1], b );\n" +" ans.z = dot3F4( a.m_row[2], b );\n" +" ans.w = 0.f;\n" +" return ans;\n" +"}\n" +"__inline\n" +"float4 mtMul3(float4 a, Matrix3x3 b)\n" +"{\n" +" float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" +" float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" +" float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" +" float4 ans;\n" +" ans.x = dot3F4( a, colx );\n" +" ans.y = dot3F4( a, coly );\n" +" ans.z = dot3F4( a, colz );\n" +" return ans;\n" +"}\n" +"///////////////////////////////////////\n" +"// Quaternion\n" +"///////////////////////////////////////\n" +"typedef float4 Quaternion;\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b);\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in);\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec);\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q);\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b)\n" +"{\n" +" Quaternion ans;\n" +" ans = cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in)\n" +"{\n" +" return fastNormalize4(in);\n" +"// in /= length( in );\n" +"// return in;\n" +"}\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec)\n" +"{\n" +" Quaternion qInv = qtInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q)\n" +"{\n" +" return (Quaternion)(-q.xyz, q.w);\n" +"}\n" +"__inline\n" +"float4 qtInvRotate(const Quaternion q, float4 vec)\n" +"{\n" +" return qtRotate( qtInvert( q ), vec );\n" +"}\n" +"#define WG_SIZE 64\n" +"typedef struct\n" +"{\n" +" float4 m_pos;\n" +" Quaternion m_quat;\n" +" float4 m_linVel;\n" +" float4 m_angVel;\n" +" u32 m_shapeIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"} Body;\n" +"typedef struct\n" +"{\n" +" Matrix3x3 m_invInertia;\n" +" Matrix3x3 m_initInvInertia;\n" +"} Shape;\n" +"typedef struct\n" +"{\n" +" float4 m_linear;\n" +" float4 m_worldPos[4];\n" +" float4 m_center; \n" +" float m_jacCoeffInv[4];\n" +" float m_b[4];\n" +" float m_appliedRambdaDt[4];\n" +" float m_fJacCoeffInv[2]; \n" +" float m_fAppliedRambdaDt[2]; \n" +" u32 m_bodyA;\n" +" u32 m_bodyB;\n" +" int m_batchIdx;\n" +" u32 m_paddings[1];\n" +"} Constraint4;\n" +"typedef struct\n" +"{\n" +" int m_nConstraints;\n" +" int m_start;\n" +" int m_batchIdx;\n" +" int m_nSplit;\n" +"// int m_paddings[1];\n" +"} ConstBuffer;\n" +"typedef struct\n" +"{\n" +" int m_solveFriction;\n" +" int m_maxBatch; // long batch really kills the performance\n" +" int m_batchIdx;\n" +" int m_nSplit;\n" +"// int m_paddings[1];\n" +"} ConstBufferBatchSolve;\n" +" \n" +"typedef struct \n" +"{\n" +" int m_valInt0;\n" +" int m_valInt1;\n" +" int m_valInt2;\n" +" int m_valInt3;\n" +" float m_val0;\n" +" float m_val1;\n" +" float m_val2;\n" +" float m_val3;\n" +"} SolverDebugInfo;\n" +"// others\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void ReorderContactKernel(__global struct b3Contact4Data* in, __global struct b3Contact4Data* out, __global int2* sortData, int4 cb )\n" +"{\n" +" int nContacts = cb.x;\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" if( gIdx < nContacts )\n" +" {\n" +" int srcIdx = sortData[gIdx].y;\n" +" out[gIdx] = in[srcIdx];\n" +" }\n" +"}\n" +"__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void SetDeterminismSortDataChildShapeB(__global struct b3Contact4Data* contactsIn, __global int2* sortDataOut, int nContacts)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" if( gIdx < nContacts )\n" +" {\n" +" int2 sd;\n" +" sd.x = contactsIn[gIdx].m_childIndexB;\n" +" sd.y = gIdx;\n" +" sortDataOut[gIdx] = sd;\n" +" }\n" +"}\n" +"__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void SetDeterminismSortDataChildShapeA(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" if( gIdx < nContacts )\n" +" {\n" +" int2 sdIn;\n" +" sdIn = sortDataInOut[gIdx];\n" +" int2 sdOut;\n" +" sdOut.x = contactsIn[sdIn.y].m_childIndexA;\n" +" sdOut.y = sdIn.y;\n" +" sortDataInOut[gIdx] = sdOut;\n" +" }\n" +"}\n" +"__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void SetDeterminismSortDataBodyA(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" if( gIdx < nContacts )\n" +" {\n" +" int2 sdIn;\n" +" sdIn = sortDataInOut[gIdx];\n" +" int2 sdOut;\n" +" sdOut.x = contactsIn[sdIn.y].m_bodyAPtrAndSignBit;\n" +" sdOut.y = sdIn.y;\n" +" sortDataInOut[gIdx] = sdOut;\n" +" }\n" +"}\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void SetDeterminismSortDataBodyB(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" if( gIdx < nContacts )\n" +" {\n" +" int2 sdIn;\n" +" sdIn = sortDataInOut[gIdx];\n" +" int2 sdOut;\n" +" sdOut.x = contactsIn[sdIn.y].m_bodyBPtrAndSignBit;\n" +" sdOut.y = sdIn.y;\n" +" sortDataInOut[gIdx] = sdOut;\n" +" }\n" +"}\n" +"typedef struct\n" +"{\n" +" int m_nContacts;\n" +" int m_staticIdx;\n" +" float m_scale;\n" +" int m_nSplit;\n" +"} ConstBufferSSD;\n" +"__constant const int gridTable4x4[] = \n" +"{\n" +" 0,1,17,16,\n" +" 1,2,18,19,\n" +" 17,18,32,3,\n" +" 16,19,3,34\n" +"};\n" +"__constant const int gridTable8x8[] = \n" +"{\n" +" 0, 2, 3, 16, 17, 18, 19, 1,\n" +" 66, 64, 80, 67, 82, 81, 65, 83,\n" +" 131,144,128,130,147,129,145,146,\n" +" 208,195,194,192,193,211,210,209,\n" +" 21, 22, 23, 5, 4, 6, 7, 20,\n" +" 86, 85, 69, 87, 70, 68, 84, 71,\n" +" 151,133,149,150,135,148,132,134,\n" +" 197,27,214,213,212,199,198,196\n" +" \n" +"};\n" +"#define USE_SPATIAL_BATCHING 1\n" +"#define USE_4x4_GRID 1\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void SetSortDataKernel(__global struct b3Contact4Data* gContact, __global Body* gBodies, __global int2* gSortDataOut, \n" +"int nContacts,float scale,int4 nSplit,int staticIdx)\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" \n" +" if( gIdx < nContacts )\n" +" {\n" +" int aPtrAndSignBit = gContact[gIdx].m_bodyAPtrAndSignBit;\n" +" int bPtrAndSignBit = gContact[gIdx].m_bodyBPtrAndSignBit;\n" +" int aIdx = abs(aPtrAndSignBit );\n" +" int bIdx = abs(bPtrAndSignBit);\n" +" bool aStatic = (aPtrAndSignBit<0) ||(aPtrAndSignBit==staticIdx);\n" +" bool bStatic = (bPtrAndSignBit<0) ||(bPtrAndSignBit==staticIdx);\n" +"#if USE_SPATIAL_BATCHING \n" +" int idx = (aStatic)? bIdx: aIdx;\n" +" float4 p = gBodies[idx].m_pos;\n" +" int xIdx = (int)((p.x-((p.x<0.f)?1.f:0.f))*scale) & (nSplit.x-1);\n" +" int yIdx = (int)((p.y-((p.y<0.f)?1.f:0.f))*scale) & (nSplit.y-1);\n" +" int zIdx = (int)((p.z-((p.z<0.f)?1.f:0.f))*scale) & (nSplit.z-1);\n" +" int newIndex = (xIdx+yIdx*nSplit.x+zIdx*nSplit.x*nSplit.y);\n" +" \n" +"#else//USE_SPATIAL_BATCHING\n" +" #if USE_4x4_GRID\n" +" int aa = aIdx&3;\n" +" int bb = bIdx&3;\n" +" if (aStatic)\n" +" aa = bb;\n" +" if (bStatic)\n" +" bb = aa;\n" +" int gridIndex = aa + bb*4;\n" +" int newIndex = gridTable4x4[gridIndex];\n" +" #else//USE_4x4_GRID\n" +" int aa = aIdx&7;\n" +" int bb = bIdx&7;\n" +" if (aStatic)\n" +" aa = bb;\n" +" if (bStatic)\n" +" bb = aa;\n" +" int gridIndex = aa + bb*8;\n" +" int newIndex = gridTable8x8[gridIndex];\n" +" #endif//USE_4x4_GRID\n" +"#endif//USE_SPATIAL_BATCHING\n" +" gSortDataOut[gIdx].x = newIndex;\n" +" gSortDataOut[gIdx].y = gIdx;\n" +" }\n" +" else\n" +" {\n" +" gSortDataOut[gIdx].x = 0xffffffff;\n" +" }\n" +"}\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void CopyConstraintKernel(__global struct b3Contact4Data* gIn, __global struct b3Contact4Data* gOut, int4 cb )\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" if( gIdx < cb.x )\n" +" {\n" +" gOut[gIdx] = gIn[gIdx];\n" +" }\n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl new file mode 100644 index 000000000000..a21a08c3b4ef --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl @@ -0,0 +1,968 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, 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. +*/ +//Originally written by Erwin Coumans + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable + + +#ifdef cl_ext_atomic_counters_32 +#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable +#else +#define counter32_t volatile global int* +#endif + +typedef unsigned int u32; +typedef unsigned short u16; +typedef unsigned char u8; + +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GET_NUM_GROUPS get_num_groups(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) +#define AppendInc(x, out) out = atomic_inc(x) +#define AtomAdd(x, value) atom_add(&(x), value) +#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value ) +#define AtomXhg(x, value) atom_xchg ( &(x), value ) + + +#define SELECT_UINT4( b, a, condition ) select( b,a,condition ) + +#define make_float4 (float4) +#define make_float2 (float2) +#define make_uint4 (uint4) +#define make_int4 (int4) +#define make_uint2 (uint2) +#define make_int2 (int2) + + +#define max2 max +#define min2 min + + +/////////////////////////////////////// +// Vector +/////////////////////////////////////// +__inline +float fastDiv(float numerator, float denominator) +{ + return native_divide(numerator, denominator); +// return numerator/denominator; +} + +__inline +float4 fastDiv4(float4 numerator, float4 denominator) +{ + return native_divide(numerator, denominator); +} + +__inline +float fastSqrtf(float f2) +{ + return native_sqrt(f2); +// return sqrt(f2); +} + +__inline +float fastRSqrt(float f2) +{ + return native_rsqrt(f2); +} + +__inline +float fastLength4(float4 v) +{ + return fast_length(v); +} + +__inline +float4 fastNormalize4(float4 v) +{ + return fast_normalize(v); +} + + +__inline +float sqrtf(float a) +{ +// return sqrt(a); + return native_sqrt(a); +} + +__inline +float4 cross3(float4 a1, float4 b1) +{ + + float4 a=make_float4(a1.xyz,0.f); + float4 b=make_float4(b1.xyz,0.f); + //float4 a=a1; + //float4 b=b1; + return cross(a,b); +} + +__inline +float dot3F4(float4 a, float4 b) +{ + float4 a1 = make_float4(a.xyz,0.f); + float4 b1 = make_float4(b.xyz,0.f); + return dot(a1, b1); +} + +__inline +float length3(const float4 a) +{ + return sqrtf(dot3F4(a,a)); +} + +__inline +float dot4(const float4 a, const float4 b) +{ + return dot( a, b ); +} + +// for height +__inline +float dot3w1(const float4 point, const float4 eqn) +{ + return dot3F4(point,eqn) + eqn.w; +} + +__inline +float4 normalize3(const float4 a) +{ + float4 n = make_float4(a.x, a.y, a.z, 0.f); + return fastNormalize4( n ); +// float length = sqrtf(dot3F4(a, a)); +// return 1.f/length * a; +} + +__inline +float4 normalize4(const float4 a) +{ + float length = sqrtf(dot4(a, a)); + return 1.f/length * a; +} + +__inline +float4 createEquation(const float4 a, const float4 b, const float4 c) +{ + float4 eqn; + float4 ab = b-a; + float4 ac = c-a; + eqn = normalize3( cross3(ab, ac) ); + eqn.w = -dot3F4(eqn,a); + return eqn; +} + +/////////////////////////////////////// +// Matrix3x3 +/////////////////////////////////////// + +typedef struct +{ + float4 m_row[3]; +}Matrix3x3; + +__inline +Matrix3x3 mtZero(); + +__inline +Matrix3x3 mtIdentity(); + +__inline +Matrix3x3 mtTranspose(Matrix3x3 m); + +__inline +Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b); + +__inline +float4 mtMul1(Matrix3x3 a, float4 b); + +__inline +float4 mtMul3(float4 a, Matrix3x3 b); + +__inline +Matrix3x3 mtZero() +{ + Matrix3x3 m; + m.m_row[0] = (float4)(0.f); + m.m_row[1] = (float4)(0.f); + m.m_row[2] = (float4)(0.f); + return m; +} + +__inline +Matrix3x3 mtIdentity() +{ + Matrix3x3 m; + m.m_row[0] = (float4)(1,0,0,0); + m.m_row[1] = (float4)(0,1,0,0); + m.m_row[2] = (float4)(0,0,1,0); + return m; +} + +__inline +Matrix3x3 mtTranspose(Matrix3x3 m) +{ + Matrix3x3 out; + out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f); + out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f); + out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f); + return out; +} + +__inline +Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b) +{ + Matrix3x3 transB; + transB = mtTranspose( b ); + Matrix3x3 ans; + // why this doesn't run when 0ing in the for{} + a.m_row[0].w = 0.f; + a.m_row[1].w = 0.f; + a.m_row[2].w = 0.f; + for(int i=0; i<3; i++) + { +// a.m_row[i].w = 0.f; + ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]); + ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]); + ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]); + ans.m_row[i].w = 0.f; + } + return ans; +} + +__inline +float4 mtMul1(Matrix3x3 a, float4 b) +{ + float4 ans; + ans.x = dot3F4( a.m_row[0], b ); + ans.y = dot3F4( a.m_row[1], b ); + ans.z = dot3F4( a.m_row[2], b ); + ans.w = 0.f; + return ans; +} + +__inline +float4 mtMul3(float4 a, Matrix3x3 b) +{ + float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0); + float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0); + float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0); + + float4 ans; + ans.x = dot3F4( a, colx ); + ans.y = dot3F4( a, coly ); + ans.z = dot3F4( a, colz ); + return ans; +} + +/////////////////////////////////////// +// Quaternion +/////////////////////////////////////// + +typedef float4 Quaternion; + +__inline +Quaternion qtMul(Quaternion a, Quaternion b); + +__inline +Quaternion qtNormalize(Quaternion in); + +__inline +float4 qtRotate(Quaternion q, float4 vec); + +__inline +Quaternion qtInvert(Quaternion q); + + + + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b) +{ + Quaternion ans; + ans = cross3( a, b ); + ans += a.w*b+b.w*a; +// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - dot3F4(a, b); + return ans; +} + +__inline +Quaternion qtNormalize(Quaternion in) +{ + return fastNormalize4(in); +// in /= length( in ); +// return in; +} +__inline +float4 qtRotate(Quaternion q, float4 vec) +{ + Quaternion qInv = qtInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = qtMul(qtMul(q,vcpy),qInv); + return out; +} + +__inline +Quaternion qtInvert(Quaternion q) +{ + return (Quaternion)(-q.xyz, q.w); +} + +__inline +float4 qtInvRotate(const Quaternion q, float4 vec) +{ + return qtRotate( qtInvert( q ), vec ); +} + + + + +#define WG_SIZE 64 + +typedef struct +{ + float4 m_pos; + Quaternion m_quat; + float4 m_linVel; + float4 m_angVel; + + u32 m_shapeIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} Body; + + + +typedef struct +{ + Matrix3x3 m_invInertia; + Matrix3x3 m_initInvInertia; +} Shape; + +typedef struct +{ + float4 m_linear; + float4 m_worldPos[4]; + float4 m_center; + float m_jacCoeffInv[4]; + float m_b[4]; + float m_appliedRambdaDt[4]; + + float m_fJacCoeffInv[2]; + float m_fAppliedRambdaDt[2]; + + u32 m_bodyA; + u32 m_bodyB; + int m_batchIdx; + u32 m_paddings; +} Constraint4; + + + + + + +__kernel void CountBodiesKernel(__global struct b3Contact4Data* manifoldPtr, __global unsigned int* bodyCount, __global int2* contactConstraintOffsets, int numContactManifolds, int fixedBodyIndex) +{ + int i = GET_GLOBAL_IDX; + + if( i < numContactManifolds) + { + int pa = manifoldPtr[i].m_bodyAPtrAndSignBit; + bool isFixedA = (pa <0) || (pa == fixedBodyIndex); + int bodyIndexA = abs(pa); + if (!isFixedA) + { + AtomInc1(bodyCount[bodyIndexA],contactConstraintOffsets[i].x); + } + barrier(CLK_GLOBAL_MEM_FENCE); + int pb = manifoldPtr[i].m_bodyBPtrAndSignBit; + bool isFixedB = (pb <0) || (pb == fixedBodyIndex); + int bodyIndexB = abs(pb); + if (!isFixedB) + { + AtomInc1(bodyCount[bodyIndexB],contactConstraintOffsets[i].y); + } + } +} + +__kernel void ClearVelocitiesKernel(__global float4* linearVelocities,__global float4* angularVelocities, int numSplitBodies) +{ + int i = GET_GLOBAL_IDX; + + if( i < numSplitBodies) + { + linearVelocities[i] = make_float4(0); + angularVelocities[i] = make_float4(0); + } +} + + +__kernel void AverageVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount, +__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies) +{ + int i = GET_GLOBAL_IDX; + if (i 0.70710678f) { + // choose p in y-z plane + float a = n.y*n.y + n.z*n.z; + float k = 1.f/sqrt(a); + p[0].x = 0; + p[0].y = -n.z*k; + p[0].z = n.y*k; + // set q = n x p + q[0].x = a*k; + q[0].y = -n.x*p[0].z; + q[0].z = n.x*p[0].y; + } + else { + // choose p in x-y plane + float a = n.x*n.x + n.y*n.y; + float k = 1.f/sqrt(a); + p[0].x = -n.y*k; + p[0].y = n.x*k; + p[0].z = 0; + // set q = n x p + q[0].x = -n.z*p[0].y; + q[0].y = n.z*p[0].x; + q[0].z = a*k; + } +} + + + + + +void solveContact(__global Constraint4* cs, + float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA, + float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB, + float4* dLinVelA, float4* dAngVelA, float4* dLinVelB, float4* dAngVelB) +{ + float minRambdaDt = 0; + float maxRambdaDt = FLT_MAX; + + for(int ic=0; ic<4; ic++) + { + if( cs->m_jacCoeffInv[ic] == 0.f ) continue; + + float4 angular0, angular1, linear; + float4 r0 = cs->m_worldPos[ic] - posA; + float4 r1 = cs->m_worldPos[ic] - posB; + setLinearAndAngular( cs->m_linear, r0, r1, &linear, &angular0, &angular1 ); + + + + float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, + *linVelA+*dLinVelA, *angVelA+*dAngVelA, *linVelB+*dLinVelB, *angVelB+*dAngVelB ) + cs->m_b[ic]; + rambdaDt *= cs->m_jacCoeffInv[ic]; + + + { + float prevSum = cs->m_appliedRambdaDt[ic]; + float updated = prevSum; + updated += rambdaDt; + updated = max2( updated, minRambdaDt ); + updated = min2( updated, maxRambdaDt ); + rambdaDt = updated - prevSum; + cs->m_appliedRambdaDt[ic] = updated; + } + + + float4 linImp0 = invMassA*linear*rambdaDt; + float4 linImp1 = invMassB*(-linear)*rambdaDt; + float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt; + float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt; + + + if (invMassA) + { + *dLinVelA += linImp0; + *dAngVelA += angImp0; + } + if (invMassB) + { + *dLinVelB += linImp1; + *dAngVelB += angImp1; + } + } +} + + +// solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,contactConstraintOffsets,offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities); + + +void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs, +__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies, +__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities) +{ + + //float frictionCoeff = ldsCs[0].m_linear.w; + int aIdx = ldsCs[0].m_bodyA; + int bIdx = ldsCs[0].m_bodyB; + + float4 posA = gBodies[aIdx].m_pos; + float4 linVelA = gBodies[aIdx].m_linVel; + float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia; + + float4 posB = gBodies[bIdx].m_pos; + float4 linVelB = gBodies[bIdx].m_linVel; + float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia; + + + float4 dLinVelA = make_float4(0,0,0,0); + float4 dAngVelA = make_float4(0,0,0,0); + float4 dLinVelB = make_float4(0,0,0,0); + float4 dAngVelB = make_float4(0,0,0,0); + + int bodyOffsetA = offsetSplitBodies[aIdx]; + int constraintOffsetA = contactConstraintOffsets[0].x; + int splitIndexA = bodyOffsetA+constraintOffsetA; + + if (invMassA) + { + dLinVelA = deltaLinearVelocities[splitIndexA]; + dAngVelA = deltaAngularVelocities[splitIndexA]; + } + + int bodyOffsetB = offsetSplitBodies[bIdx]; + int constraintOffsetB = contactConstraintOffsets[0].y; + int splitIndexB= bodyOffsetB+constraintOffsetB; + + if (invMassB) + { + dLinVelB = deltaLinearVelocities[splitIndexB]; + dAngVelB = deltaAngularVelocities[splitIndexB]; + } + + solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA, + posB, &linVelB, &angVelB, invMassB, invInertiaB ,&dLinVelA, &dAngVelA, &dLinVelB, &dAngVelB); + + if (invMassA) + { + deltaLinearVelocities[splitIndexA] = dLinVelA; + deltaAngularVelocities[splitIndexA] = dAngVelA; + } + if (invMassB) + { + deltaLinearVelocities[splitIndexB] = dLinVelB; + deltaAngularVelocities[splitIndexB] = dAngVelB; + } + +} + + +__kernel void SolveContactJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes , +__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, +float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds +) +{ + int i = GET_GLOBAL_IDX; + if (im_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return; + const float4 center = cs->m_center; + + float4 n = -cs->m_linear; + + float4 tangent[2]; + btPlaneSpace1(n,&tangent[0],&tangent[1]); + float4 angular0, angular1, linear; + float4 r0 = center - posA; + float4 r1 = center - posB; + for(int i=0; i<2; i++) + { + setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 ); + float rambdaDt = calcRelVel(linear, -linear, angular0, angular1, + linVelA+dLinVelA, angVelA+dAngVelA, linVelB+dLinVelB, angVelB+dAngVelB ); + rambdaDt *= cs->m_fJacCoeffInv[i]; + + { + float prevSum = cs->m_fAppliedRambdaDt[i]; + float updated = prevSum; + updated += rambdaDt; + updated = max2( updated, minRambdaDt[i] ); + updated = min2( updated, maxRambdaDt[i] ); + rambdaDt = updated - prevSum; + cs->m_fAppliedRambdaDt[i] = updated; + } + + float4 linImp0 = invMassA*linear*rambdaDt; + float4 linImp1 = invMassB*(-linear)*rambdaDt; + float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt; + float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt; + + dLinVelA += linImp0; + dAngVelA += angImp0; + dLinVelB += linImp1; + dAngVelB += angImp1; + } + { // angular damping for point constraint + float4 ab = normalize3( posB - posA ); + float4 ac = normalize3( center - posA ); + if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) + { + float angNA = dot3F4( n, angVelA ); + float angNB = dot3F4( n, angVelB ); + + dAngVelA -= (angNA*0.1f)*n; + dAngVelB -= (angNB*0.1f)*n; + } + } + } + + + + } + + if (invMassA) + { + deltaLinearVelocities[splitIndexA] = dLinVelA; + deltaAngularVelocities[splitIndexA] = dAngVelA; + } + if (invMassB) + { + deltaLinearVelocities[splitIndexB] = dLinVelB; + deltaAngularVelocities[splitIndexB] = dAngVelB; + } + + +} + + +__kernel void SolveFrictionJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes , + __global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies, + __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, + float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds +) +{ + int i = GET_GLOBAL_IDX; + if (im_bodyA = abs(src->m_bodyAPtrAndSignBit); + dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit); + + float dtInv = 1.f/dt; + for(int ic=0; ic<4; ic++) + { + dstC->m_appliedRambdaDt[ic] = 0.f; + } + dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f; + + + dstC->m_linear = src->m_worldNormalOnB; + dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() ); + for(int ic=0; ic<4; ic++) + { + float4 r0 = src->m_worldPosB[ic] - posA; + float4 r1 = src->m_worldPosB[ic] - posB; + + if( ic >= src->m_worldNormalOnB.w )//npoints + { + dstC->m_jacCoeffInv[ic] = 0.f; + continue; + } + + float relVelN; + { + float4 linear, angular0, angular1; + setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1); + + dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1, + invMassA, &invInertiaA, invMassB, &invInertiaB , countA, countB); + + relVelN = calcRelVel(linear, -linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB); + + float e = 0.f;//src->getRestituitionCoeff(); + if( relVelN*relVelN < 0.004f ) e = 0.f; + + dstC->m_b[ic] = e*relVelN; + //float penetration = src->m_worldPosB[ic].w; + dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv; + dstC->m_appliedRambdaDt[ic] = 0.f; + } + } + + if( src->m_worldNormalOnB.w > 0 )//npoints + { // prepare friction + float4 center = make_float4(0.f); + for(int i=0; im_worldNormalOnB.w; i++) + center += src->m_worldPosB[i]; + center /= (float)src->m_worldNormalOnB.w; + + float4 tangent[2]; + btPlaneSpace1(-src->m_worldNormalOnB,&tangent[0],&tangent[1]); + + float4 r[2]; + r[0] = center - posA; + r[1] = center - posB; + + for(int i=0; i<2; i++) + { + float4 linear, angular0, angular1; + setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1); + + dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1, + invMassA, &invInertiaA, invMassB, &invInertiaB ,countA, countB); + dstC->m_fAppliedRambdaDt[i] = 0.f; + } + dstC->m_center = center; + } + + for(int i=0; i<4; i++) + { + if( im_worldNormalOnB.w ) + { + dstC->m_worldPos[i] = src->m_worldPosB[i]; + } + else + { + dstC->m_worldPos[i] = make_float4(0.f); + } + } +} + + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void ContactToConstraintSplitKernel(__global const struct b3Contact4Data* gContact, __global const Body* gBodies, __global const Shape* gShapes, __global Constraint4* gConstraintOut, +__global const unsigned int* bodyCount, +int nContacts, +float dt, +float positionDrift, +float positionConstraintCoeff +) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit); + int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit); + + float4 posA = gBodies[aIdx].m_pos; + float4 linVelA = gBodies[aIdx].m_linVel; + float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia; + + float4 posB = gBodies[bIdx].m_pos; + float4 linVelB = gBodies[bIdx].m_linVel; + float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia; + + Constraint4 cs; + + float countA = invMassA != 0.f ? (float)bodyCount[aIdx] : 1; + float countB = invMassB != 0.f ? (float)bodyCount[bIdx] : 1; + + setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB, + &gContact[gIdx], dt, positionDrift, positionConstraintCoeff,countA,countB, + &cs ); + + cs.m_batchIdx = gContact[gIdx].m_batchIdx; + + gConstraintOut[gIdx] = cs; + } +} \ No newline at end of file diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h new file mode 100644 index 000000000000..c0173ad9f42a --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h @@ -0,0 +1,909 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* solverUtilsCL= \ +"/*\n" +"Copyright (c) 2013 Advanced Micro Devices, Inc. \n" +"This software is provided 'as-is', without any express or implied warranty.\n" +"In no event will the authors be held liable for any damages arising from the use of this software.\n" +"Permission is granted to anyone to use this software for any purpose, \n" +"including commercial applications, and to alter it and redistribute it freely, \n" +"subject to the following restrictions:\n" +"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.\n" +"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" +"3. This notice may not be removed or altered from any source distribution.\n" +"*/\n" +"//Originally written by Erwin Coumans\n" +"#ifndef B3_CONTACT4DATA_H\n" +"#define B3_CONTACT4DATA_H\n" +"#ifndef B3_FLOAT4_H\n" +"#define B3_FLOAT4_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#define B3_PLATFORM_DEFINITIONS_H\n" +"struct MyTest\n" +"{\n" +" int bla;\n" +"};\n" +"#ifdef __cplusplus\n" +"#else\n" +"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" +"#define B3_LARGE_FLOAT 1e18f\n" +"#define B3_INFINITY 1e18f\n" +"#define b3Assert(a)\n" +"#define b3ConstArray(a) __global const a*\n" +"#define b3AtomicInc atomic_inc\n" +"#define b3AtomicAdd atomic_add\n" +"#define b3Fabs fabs\n" +"#define b3Sqrt native_sqrt\n" +"#define b3Sin native_sin\n" +"#define b3Cos native_cos\n" +"#define B3_STATIC\n" +"#endif\n" +"#endif\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Float4;\n" +" #define b3Float4ConstArg const b3Float4\n" +" #define b3MakeFloat4 (float4)\n" +" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return dot(a1, b1);\n" +" }\n" +" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return cross(a1, b1);\n" +" }\n" +" #define b3MinFloat4 min\n" +" #define b3MaxFloat4 max\n" +" #define b3Normalized(a) normalize(a)\n" +"#endif \n" +" \n" +"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" +"{\n" +" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" +" return false;\n" +" return true;\n" +"}\n" +"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" +"{\n" +" float maxDot = -B3_INFINITY;\n" +" int i = 0;\n" +" int ptIndex = -1;\n" +" for( i = 0; i < vecLen; i++ )\n" +" {\n" +" float dot = b3Dot3F4(vecArray[i],vec);\n" +" \n" +" if( dot > maxDot )\n" +" {\n" +" maxDot = dot;\n" +" ptIndex = i;\n" +" }\n" +" }\n" +" b3Assert(ptIndex>=0);\n" +" if (ptIndex<0)\n" +" {\n" +" ptIndex = 0;\n" +" }\n" +" *dotOut = maxDot;\n" +" return ptIndex;\n" +"}\n" +"#endif //B3_FLOAT4_H\n" +"typedef struct b3Contact4Data b3Contact4Data_t;\n" +"struct b3Contact4Data\n" +"{\n" +" b3Float4 m_worldPosB[4];\n" +"// b3Float4 m_localPosA[4];\n" +"// b3Float4 m_localPosB[4];\n" +" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" +" unsigned short m_restituitionCoeffCmp;\n" +" unsigned short m_frictionCoeffCmp;\n" +" int m_batchIdx;\n" +" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" +" int m_bodyBPtrAndSignBit;\n" +" int m_childIndexA;\n" +" int m_childIndexB;\n" +" int m_unused1;\n" +" int m_unused2;\n" +"};\n" +"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" +"{\n" +" return (int)contact->m_worldNormalOnB.w;\n" +"};\n" +"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" +"{\n" +" contact->m_worldNormalOnB.w = (float)numPoints;\n" +"};\n" +"#endif //B3_CONTACT4DATA_H\n" +"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" +"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" +"#ifdef cl_ext_atomic_counters_32\n" +"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" +"#else\n" +"#define counter32_t volatile global int*\n" +"#endif\n" +"typedef unsigned int u32;\n" +"typedef unsigned short u16;\n" +"typedef unsigned char u8;\n" +"#define GET_GROUP_IDX get_group_id(0)\n" +"#define GET_LOCAL_IDX get_local_id(0)\n" +"#define GET_GLOBAL_IDX get_global_id(0)\n" +"#define GET_GROUP_SIZE get_local_size(0)\n" +"#define GET_NUM_GROUPS get_num_groups(0)\n" +"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" +"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" +"#define AtomInc(x) atom_inc(&(x))\n" +"#define AtomInc1(x, out) out = atom_inc(&(x))\n" +"#define AppendInc(x, out) out = atomic_inc(x)\n" +"#define AtomAdd(x, value) atom_add(&(x), value)\n" +"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" +"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" +"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" +"#define make_float4 (float4)\n" +"#define make_float2 (float2)\n" +"#define make_uint4 (uint4)\n" +"#define make_int4 (int4)\n" +"#define make_uint2 (uint2)\n" +"#define make_int2 (int2)\n" +"#define max2 max\n" +"#define min2 min\n" +"///////////////////////////////////////\n" +"// Vector\n" +"///////////////////////////////////////\n" +"__inline\n" +"float fastDiv(float numerator, float denominator)\n" +"{\n" +" return native_divide(numerator, denominator); \n" +"// return numerator/denominator; \n" +"}\n" +"__inline\n" +"float4 fastDiv4(float4 numerator, float4 denominator)\n" +"{\n" +" return native_divide(numerator, denominator); \n" +"}\n" +"__inline\n" +"float fastSqrtf(float f2)\n" +"{\n" +" return native_sqrt(f2);\n" +"// return sqrt(f2);\n" +"}\n" +"__inline\n" +"float fastRSqrt(float f2)\n" +"{\n" +" return native_rsqrt(f2);\n" +"}\n" +"__inline\n" +"float fastLength4(float4 v)\n" +"{\n" +" return fast_length(v);\n" +"}\n" +"__inline\n" +"float4 fastNormalize4(float4 v)\n" +"{\n" +" return fast_normalize(v);\n" +"}\n" +"__inline\n" +"float sqrtf(float a)\n" +"{\n" +"// return sqrt(a);\n" +" return native_sqrt(a);\n" +"}\n" +"__inline\n" +"float4 cross3(float4 a1, float4 b1)\n" +"{\n" +" float4 a=make_float4(a1.xyz,0.f);\n" +" float4 b=make_float4(b1.xyz,0.f);\n" +" //float4 a=a1;\n" +" //float4 b=b1;\n" +" return cross(a,b);\n" +"}\n" +"__inline\n" +"float dot3F4(float4 a, float4 b)\n" +"{\n" +" float4 a1 = make_float4(a.xyz,0.f);\n" +" float4 b1 = make_float4(b.xyz,0.f);\n" +" return dot(a1, b1);\n" +"}\n" +"__inline\n" +"float length3(const float4 a)\n" +"{\n" +" return sqrtf(dot3F4(a,a));\n" +"}\n" +"__inline\n" +"float dot4(const float4 a, const float4 b)\n" +"{\n" +" return dot( a, b );\n" +"}\n" +"// for height\n" +"__inline\n" +"float dot3w1(const float4 point, const float4 eqn)\n" +"{\n" +" return dot3F4(point,eqn) + eqn.w;\n" +"}\n" +"__inline\n" +"float4 normalize3(const float4 a)\n" +"{\n" +" float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" +" return fastNormalize4( n );\n" +"// float length = sqrtf(dot3F4(a, a));\n" +"// return 1.f/length * a;\n" +"}\n" +"__inline\n" +"float4 normalize4(const float4 a)\n" +"{\n" +" float length = sqrtf(dot4(a, a));\n" +" return 1.f/length * a;\n" +"}\n" +"__inline\n" +"float4 createEquation(const float4 a, const float4 b, const float4 c)\n" +"{\n" +" float4 eqn;\n" +" float4 ab = b-a;\n" +" float4 ac = c-a;\n" +" eqn = normalize3( cross3(ab, ac) );\n" +" eqn.w = -dot3F4(eqn,a);\n" +" return eqn;\n" +"}\n" +"///////////////////////////////////////\n" +"// Matrix3x3\n" +"///////////////////////////////////////\n" +"typedef struct\n" +"{\n" +" float4 m_row[3];\n" +"}Matrix3x3;\n" +"__inline\n" +"Matrix3x3 mtZero();\n" +"__inline\n" +"Matrix3x3 mtIdentity();\n" +"__inline\n" +"Matrix3x3 mtTranspose(Matrix3x3 m);\n" +"__inline\n" +"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b);\n" +"__inline\n" +"float4 mtMul1(Matrix3x3 a, float4 b);\n" +"__inline\n" +"float4 mtMul3(float4 a, Matrix3x3 b);\n" +"__inline\n" +"Matrix3x3 mtZero()\n" +"{\n" +" Matrix3x3 m;\n" +" m.m_row[0] = (float4)(0.f);\n" +" m.m_row[1] = (float4)(0.f);\n" +" m.m_row[2] = (float4)(0.f);\n" +" return m;\n" +"}\n" +"__inline\n" +"Matrix3x3 mtIdentity()\n" +"{\n" +" Matrix3x3 m;\n" +" m.m_row[0] = (float4)(1,0,0,0);\n" +" m.m_row[1] = (float4)(0,1,0,0);\n" +" m.m_row[2] = (float4)(0,0,1,0);\n" +" return m;\n" +"}\n" +"__inline\n" +"Matrix3x3 mtTranspose(Matrix3x3 m)\n" +"{\n" +" Matrix3x3 out;\n" +" out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" +" out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" +" out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" +" return out;\n" +"}\n" +"__inline\n" +"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)\n" +"{\n" +" Matrix3x3 transB;\n" +" transB = mtTranspose( b );\n" +" Matrix3x3 ans;\n" +" // why this doesn't run when 0ing in the for{}\n" +" a.m_row[0].w = 0.f;\n" +" a.m_row[1].w = 0.f;\n" +" a.m_row[2].w = 0.f;\n" +" for(int i=0; i<3; i++)\n" +" {\n" +"// a.m_row[i].w = 0.f;\n" +" ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);\n" +" ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);\n" +" ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);\n" +" ans.m_row[i].w = 0.f;\n" +" }\n" +" return ans;\n" +"}\n" +"__inline\n" +"float4 mtMul1(Matrix3x3 a, float4 b)\n" +"{\n" +" float4 ans;\n" +" ans.x = dot3F4( a.m_row[0], b );\n" +" ans.y = dot3F4( a.m_row[1], b );\n" +" ans.z = dot3F4( a.m_row[2], b );\n" +" ans.w = 0.f;\n" +" return ans;\n" +"}\n" +"__inline\n" +"float4 mtMul3(float4 a, Matrix3x3 b)\n" +"{\n" +" float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" +" float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" +" float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" +" float4 ans;\n" +" ans.x = dot3F4( a, colx );\n" +" ans.y = dot3F4( a, coly );\n" +" ans.z = dot3F4( a, colz );\n" +" return ans;\n" +"}\n" +"///////////////////////////////////////\n" +"// Quaternion\n" +"///////////////////////////////////////\n" +"typedef float4 Quaternion;\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b);\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in);\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec);\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q);\n" +"__inline\n" +"Quaternion qtMul(Quaternion a, Quaternion b)\n" +"{\n" +" Quaternion ans;\n" +" ans = cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"__inline\n" +"Quaternion qtNormalize(Quaternion in)\n" +"{\n" +" return fastNormalize4(in);\n" +"// in /= length( in );\n" +"// return in;\n" +"}\n" +"__inline\n" +"float4 qtRotate(Quaternion q, float4 vec)\n" +"{\n" +" Quaternion qInv = qtInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"__inline\n" +"Quaternion qtInvert(Quaternion q)\n" +"{\n" +" return (Quaternion)(-q.xyz, q.w);\n" +"}\n" +"__inline\n" +"float4 qtInvRotate(const Quaternion q, float4 vec)\n" +"{\n" +" return qtRotate( qtInvert( q ), vec );\n" +"}\n" +"#define WG_SIZE 64\n" +"typedef struct\n" +"{\n" +" float4 m_pos;\n" +" Quaternion m_quat;\n" +" float4 m_linVel;\n" +" float4 m_angVel;\n" +" u32 m_shapeIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"} Body;\n" +"typedef struct\n" +"{\n" +" Matrix3x3 m_invInertia;\n" +" Matrix3x3 m_initInvInertia;\n" +"} Shape;\n" +"typedef struct\n" +"{\n" +" float4 m_linear;\n" +" float4 m_worldPos[4];\n" +" float4 m_center; \n" +" float m_jacCoeffInv[4];\n" +" float m_b[4];\n" +" float m_appliedRambdaDt[4];\n" +" float m_fJacCoeffInv[2]; \n" +" float m_fAppliedRambdaDt[2]; \n" +" u32 m_bodyA;\n" +" u32 m_bodyB;\n" +" int m_batchIdx;\n" +" u32 m_paddings;\n" +"} Constraint4;\n" +"__kernel void CountBodiesKernel(__global struct b3Contact4Data* manifoldPtr, __global unsigned int* bodyCount, __global int2* contactConstraintOffsets, int numContactManifolds, int fixedBodyIndex)\n" +"{\n" +" int i = GET_GLOBAL_IDX;\n" +" \n" +" if( i < numContactManifolds)\n" +" {\n" +" int pa = manifoldPtr[i].m_bodyAPtrAndSignBit;\n" +" bool isFixedA = (pa <0) || (pa == fixedBodyIndex);\n" +" int bodyIndexA = abs(pa);\n" +" if (!isFixedA)\n" +" {\n" +" AtomInc1(bodyCount[bodyIndexA],contactConstraintOffsets[i].x);\n" +" }\n" +" barrier(CLK_GLOBAL_MEM_FENCE);\n" +" int pb = manifoldPtr[i].m_bodyBPtrAndSignBit;\n" +" bool isFixedB = (pb <0) || (pb == fixedBodyIndex);\n" +" int bodyIndexB = abs(pb);\n" +" if (!isFixedB)\n" +" {\n" +" AtomInc1(bodyCount[bodyIndexB],contactConstraintOffsets[i].y);\n" +" } \n" +" }\n" +"}\n" +"__kernel void ClearVelocitiesKernel(__global float4* linearVelocities,__global float4* angularVelocities, int numSplitBodies)\n" +"{\n" +" int i = GET_GLOBAL_IDX;\n" +" \n" +" if( i < numSplitBodies)\n" +" {\n" +" linearVelocities[i] = make_float4(0);\n" +" angularVelocities[i] = make_float4(0);\n" +" }\n" +"}\n" +"__kernel void AverageVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount,\n" +"__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies)\n" +"{\n" +" int i = GET_GLOBAL_IDX;\n" +" if (i 0.70710678f) {\n" +" // choose p in y-z plane\n" +" float a = n.y*n.y + n.z*n.z;\n" +" float k = 1.f/sqrt(a);\n" +" p[0].x = 0;\n" +" p[0].y = -n.z*k;\n" +" p[0].z = n.y*k;\n" +" // set q = n x p\n" +" q[0].x = a*k;\n" +" q[0].y = -n.x*p[0].z;\n" +" q[0].z = n.x*p[0].y;\n" +" }\n" +" else {\n" +" // choose p in x-y plane\n" +" float a = n.x*n.x + n.y*n.y;\n" +" float k = 1.f/sqrt(a);\n" +" p[0].x = -n.y*k;\n" +" p[0].y = n.x*k;\n" +" p[0].z = 0;\n" +" // set q = n x p\n" +" q[0].x = -n.z*p[0].y;\n" +" q[0].y = n.z*p[0].x;\n" +" q[0].z = a*k;\n" +" }\n" +"}\n" +"void solveContact(__global Constraint4* cs,\n" +" float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,\n" +" float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB,\n" +" float4* dLinVelA, float4* dAngVelA, float4* dLinVelB, float4* dAngVelB)\n" +"{\n" +" float minRambdaDt = 0;\n" +" float maxRambdaDt = FLT_MAX;\n" +" for(int ic=0; ic<4; ic++)\n" +" {\n" +" if( cs->m_jacCoeffInv[ic] == 0.f ) continue;\n" +" float4 angular0, angular1, linear;\n" +" float4 r0 = cs->m_worldPos[ic] - posA;\n" +" float4 r1 = cs->m_worldPos[ic] - posB;\n" +" setLinearAndAngular( cs->m_linear, r0, r1, &linear, &angular0, &angular1 );\n" +" \n" +" float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, \n" +" *linVelA+*dLinVelA, *angVelA+*dAngVelA, *linVelB+*dLinVelB, *angVelB+*dAngVelB ) + cs->m_b[ic];\n" +" rambdaDt *= cs->m_jacCoeffInv[ic];\n" +" \n" +" {\n" +" float prevSum = cs->m_appliedRambdaDt[ic];\n" +" float updated = prevSum;\n" +" updated += rambdaDt;\n" +" updated = max2( updated, minRambdaDt );\n" +" updated = min2( updated, maxRambdaDt );\n" +" rambdaDt = updated - prevSum;\n" +" cs->m_appliedRambdaDt[ic] = updated;\n" +" }\n" +" \n" +" float4 linImp0 = invMassA*linear*rambdaDt;\n" +" float4 linImp1 = invMassB*(-linear)*rambdaDt;\n" +" float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n" +" float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n" +" \n" +" if (invMassA)\n" +" {\n" +" *dLinVelA += linImp0;\n" +" *dAngVelA += angImp0;\n" +" }\n" +" if (invMassB)\n" +" {\n" +" *dLinVelB += linImp1;\n" +" *dAngVelB += angImp1;\n" +" }\n" +" }\n" +"}\n" +"// solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,contactConstraintOffsets,offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);\n" +"void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs, \n" +"__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,\n" +"__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities)\n" +"{\n" +" //float frictionCoeff = ldsCs[0].m_linear.w;\n" +" int aIdx = ldsCs[0].m_bodyA;\n" +" int bIdx = ldsCs[0].m_bodyB;\n" +" float4 posA = gBodies[aIdx].m_pos;\n" +" float4 linVelA = gBodies[aIdx].m_linVel;\n" +" float4 angVelA = gBodies[aIdx].m_angVel;\n" +" float invMassA = gBodies[aIdx].m_invMass;\n" +" Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" +" float4 posB = gBodies[bIdx].m_pos;\n" +" float4 linVelB = gBodies[bIdx].m_linVel;\n" +" float4 angVelB = gBodies[bIdx].m_angVel;\n" +" float invMassB = gBodies[bIdx].m_invMass;\n" +" Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" +" \n" +" float4 dLinVelA = make_float4(0,0,0,0);\n" +" float4 dAngVelA = make_float4(0,0,0,0);\n" +" float4 dLinVelB = make_float4(0,0,0,0);\n" +" float4 dAngVelB = make_float4(0,0,0,0);\n" +" \n" +" int bodyOffsetA = offsetSplitBodies[aIdx];\n" +" int constraintOffsetA = contactConstraintOffsets[0].x;\n" +" int splitIndexA = bodyOffsetA+constraintOffsetA;\n" +" \n" +" if (invMassA)\n" +" {\n" +" dLinVelA = deltaLinearVelocities[splitIndexA];\n" +" dAngVelA = deltaAngularVelocities[splitIndexA];\n" +" }\n" +" int bodyOffsetB = offsetSplitBodies[bIdx];\n" +" int constraintOffsetB = contactConstraintOffsets[0].y;\n" +" int splitIndexB= bodyOffsetB+constraintOffsetB;\n" +" if (invMassB)\n" +" {\n" +" dLinVelB = deltaLinearVelocities[splitIndexB];\n" +" dAngVelB = deltaAngularVelocities[splitIndexB];\n" +" }\n" +" solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n" +" posB, &linVelB, &angVelB, invMassB, invInertiaB ,&dLinVelA, &dAngVelA, &dLinVelB, &dAngVelB);\n" +" if (invMassA)\n" +" {\n" +" deltaLinearVelocities[splitIndexA] = dLinVelA;\n" +" deltaAngularVelocities[splitIndexA] = dAngVelA;\n" +" } \n" +" if (invMassB)\n" +" {\n" +" deltaLinearVelocities[splitIndexB] = dLinVelB;\n" +" deltaAngularVelocities[splitIndexB] = dAngVelB;\n" +" }\n" +"}\n" +"__kernel void SolveContactJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes ,\n" +"__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,\n" +"float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds\n" +")\n" +"{\n" +" int i = GET_GLOBAL_IDX;\n" +" if (im_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return;\n" +" const float4 center = cs->m_center;\n" +" \n" +" float4 n = -cs->m_linear;\n" +" \n" +" float4 tangent[2];\n" +" btPlaneSpace1(n,&tangent[0],&tangent[1]);\n" +" float4 angular0, angular1, linear;\n" +" float4 r0 = center - posA;\n" +" float4 r1 = center - posB;\n" +" for(int i=0; i<2; i++)\n" +" {\n" +" setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 );\n" +" float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,\n" +" linVelA+dLinVelA, angVelA+dAngVelA, linVelB+dLinVelB, angVelB+dAngVelB );\n" +" rambdaDt *= cs->m_fJacCoeffInv[i];\n" +" \n" +" {\n" +" float prevSum = cs->m_fAppliedRambdaDt[i];\n" +" float updated = prevSum;\n" +" updated += rambdaDt;\n" +" updated = max2( updated, minRambdaDt[i] );\n" +" updated = min2( updated, maxRambdaDt[i] );\n" +" rambdaDt = updated - prevSum;\n" +" cs->m_fAppliedRambdaDt[i] = updated;\n" +" }\n" +" \n" +" float4 linImp0 = invMassA*linear*rambdaDt;\n" +" float4 linImp1 = invMassB*(-linear)*rambdaDt;\n" +" float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n" +" float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n" +" \n" +" dLinVelA += linImp0;\n" +" dAngVelA += angImp0;\n" +" dLinVelB += linImp1;\n" +" dAngVelB += angImp1;\n" +" }\n" +" { // angular damping for point constraint\n" +" float4 ab = normalize3( posB - posA );\n" +" float4 ac = normalize3( center - posA );\n" +" if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))\n" +" {\n" +" float angNA = dot3F4( n, angVelA );\n" +" float angNB = dot3F4( n, angVelB );\n" +" \n" +" dAngVelA -= (angNA*0.1f)*n;\n" +" dAngVelB -= (angNB*0.1f)*n;\n" +" }\n" +" }\n" +" }\n" +" \n" +" \n" +" }\n" +" if (invMassA)\n" +" {\n" +" deltaLinearVelocities[splitIndexA] = dLinVelA;\n" +" deltaAngularVelocities[splitIndexA] = dAngVelA;\n" +" } \n" +" if (invMassB)\n" +" {\n" +" deltaLinearVelocities[splitIndexB] = dLinVelB;\n" +" deltaAngularVelocities[splitIndexB] = dAngVelB;\n" +" }\n" +" \n" +"}\n" +"__kernel void SolveFrictionJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes ,\n" +" __global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,\n" +" __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,\n" +" float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds\n" +")\n" +"{\n" +" int i = GET_GLOBAL_IDX;\n" +" if (im_bodyA = abs(src->m_bodyAPtrAndSignBit);\n" +" dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);\n" +" float dtInv = 1.f/dt;\n" +" for(int ic=0; ic<4; ic++)\n" +" {\n" +" dstC->m_appliedRambdaDt[ic] = 0.f;\n" +" }\n" +" dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;\n" +" dstC->m_linear = src->m_worldNormalOnB;\n" +" dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );\n" +" for(int ic=0; ic<4; ic++)\n" +" {\n" +" float4 r0 = src->m_worldPosB[ic] - posA;\n" +" float4 r1 = src->m_worldPosB[ic] - posB;\n" +" if( ic >= src->m_worldNormalOnB.w )//npoints\n" +" {\n" +" dstC->m_jacCoeffInv[ic] = 0.f;\n" +" continue;\n" +" }\n" +" float relVelN;\n" +" {\n" +" float4 linear, angular0, angular1;\n" +" setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);\n" +" dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,\n" +" invMassA, &invInertiaA, invMassB, &invInertiaB , countA, countB);\n" +" relVelN = calcRelVel(linear, -linear, angular0, angular1,\n" +" linVelA, angVelA, linVelB, angVelB);\n" +" float e = 0.f;//src->getRestituitionCoeff();\n" +" if( relVelN*relVelN < 0.004f ) e = 0.f;\n" +" dstC->m_b[ic] = e*relVelN;\n" +" //float penetration = src->m_worldPosB[ic].w;\n" +" dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;\n" +" dstC->m_appliedRambdaDt[ic] = 0.f;\n" +" }\n" +" }\n" +" if( src->m_worldNormalOnB.w > 0 )//npoints\n" +" { // prepare friction\n" +" float4 center = make_float4(0.f);\n" +" for(int i=0; im_worldNormalOnB.w; i++) \n" +" center += src->m_worldPosB[i];\n" +" center /= (float)src->m_worldNormalOnB.w;\n" +" float4 tangent[2];\n" +" btPlaneSpace1(-src->m_worldNormalOnB,&tangent[0],&tangent[1]);\n" +" \n" +" float4 r[2];\n" +" r[0] = center - posA;\n" +" r[1] = center - posB;\n" +" for(int i=0; i<2; i++)\n" +" {\n" +" float4 linear, angular0, angular1;\n" +" setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);\n" +" dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,\n" +" invMassA, &invInertiaA, invMassB, &invInertiaB ,countA, countB);\n" +" dstC->m_fAppliedRambdaDt[i] = 0.f;\n" +" }\n" +" dstC->m_center = center;\n" +" }\n" +" for(int i=0; i<4; i++)\n" +" {\n" +" if( im_worldNormalOnB.w )\n" +" {\n" +" dstC->m_worldPos[i] = src->m_worldPosB[i];\n" +" }\n" +" else\n" +" {\n" +" dstC->m_worldPos[i] = make_float4(0.f);\n" +" }\n" +" }\n" +"}\n" +"__kernel\n" +"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" +"void ContactToConstraintSplitKernel(__global const struct b3Contact4Data* gContact, __global const Body* gBodies, __global const Shape* gShapes, __global Constraint4* gConstraintOut, \n" +"__global const unsigned int* bodyCount,\n" +"int nContacts,\n" +"float dt,\n" +"float positionDrift,\n" +"float positionConstraintCoeff\n" +")\n" +"{\n" +" int gIdx = GET_GLOBAL_IDX;\n" +" \n" +" if( gIdx < nContacts )\n" +" {\n" +" int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);\n" +" int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);\n" +" float4 posA = gBodies[aIdx].m_pos;\n" +" float4 linVelA = gBodies[aIdx].m_linVel;\n" +" float4 angVelA = gBodies[aIdx].m_angVel;\n" +" float invMassA = gBodies[aIdx].m_invMass;\n" +" Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" +" float4 posB = gBodies[bIdx].m_pos;\n" +" float4 linVelB = gBodies[bIdx].m_linVel;\n" +" float4 angVelB = gBodies[bIdx].m_angVel;\n" +" float invMassB = gBodies[bIdx].m_invMass;\n" +" Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" +" Constraint4 cs;\n" +" float countA = invMassA != 0.f ? (float)bodyCount[aIdx] : 1;\n" +" float countB = invMassB != 0.f ? (float)bodyCount[bIdx] : 1;\n" +" setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,\n" +" &gContact[gIdx], dt, positionDrift, positionConstraintCoeff,countA,countB,\n" +" &cs );\n" +" \n" +" cs.m_batchIdx = gContact[gIdx].m_batchIdx;\n" +" gConstraintOut[gIdx] = cs;\n" +" }\n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl new file mode 100644 index 000000000000..ba8ba735d05b --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl @@ -0,0 +1,22 @@ + + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h" + + +__kernel void initializeGpuAabbsFull( const int numNodes, __global b3RigidBodyData_t* gBodies,__global b3Collidable_t* collidables, __global b3Aabb_t* plocalShapeAABB, __global b3Aabb_t* pAABB) +{ + int nodeID = get_global_id(0); + if( nodeID < numNodes ) + { + b3ComputeWorldAabb(nodeID, gBodies, collidables, plocalShapeAABB,pAABB); + } +} + +__kernel void clearOverlappingPairsKernel( __global int4* pairs, int numPairs) +{ + int pairId = get_global_id(0); + if( pairId< numPairs ) + { + pairs[pairId].z = 0xffffffff; + } +} \ No newline at end of file diff --git a/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h new file mode 100644 index 000000000000..d70e74017aa2 --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h @@ -0,0 +1,483 @@ +//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project +static const char* updateAabbsKernelCL= \ +"#ifndef B3_UPDATE_AABBS_H\n" +"#define B3_UPDATE_AABBS_H\n" +"#ifndef B3_AABB_H\n" +"#define B3_AABB_H\n" +"#ifndef B3_FLOAT4_H\n" +"#define B3_FLOAT4_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#define B3_PLATFORM_DEFINITIONS_H\n" +"struct MyTest\n" +"{\n" +" int bla;\n" +"};\n" +"#ifdef __cplusplus\n" +"#else\n" +"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" +"#define B3_LARGE_FLOAT 1e18f\n" +"#define B3_INFINITY 1e18f\n" +"#define b3Assert(a)\n" +"#define b3ConstArray(a) __global const a*\n" +"#define b3AtomicInc atomic_inc\n" +"#define b3AtomicAdd atomic_add\n" +"#define b3Fabs fabs\n" +"#define b3Sqrt native_sqrt\n" +"#define b3Sin native_sin\n" +"#define b3Cos native_cos\n" +"#define B3_STATIC\n" +"#endif\n" +"#endif\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Float4;\n" +" #define b3Float4ConstArg const b3Float4\n" +" #define b3MakeFloat4 (float4)\n" +" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return dot(a1, b1);\n" +" }\n" +" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" +" {\n" +" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" +" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" +" return cross(a1, b1);\n" +" }\n" +" #define b3MinFloat4 min\n" +" #define b3MaxFloat4 max\n" +" #define b3Normalized(a) normalize(a)\n" +"#endif \n" +" \n" +"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" +"{\n" +" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" +" return false;\n" +" return true;\n" +"}\n" +"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" +"{\n" +" float maxDot = -B3_INFINITY;\n" +" int i = 0;\n" +" int ptIndex = -1;\n" +" for( i = 0; i < vecLen; i++ )\n" +" {\n" +" float dot = b3Dot3F4(vecArray[i],vec);\n" +" \n" +" if( dot > maxDot )\n" +" {\n" +" maxDot = dot;\n" +" ptIndex = i;\n" +" }\n" +" }\n" +" b3Assert(ptIndex>=0);\n" +" if (ptIndex<0)\n" +" {\n" +" ptIndex = 0;\n" +" }\n" +" *dotOut = maxDot;\n" +" return ptIndex;\n" +"}\n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_MAT3x3_H\n" +"#define B3_MAT3x3_H\n" +"#ifndef B3_QUAT_H\n" +"#define B3_QUAT_H\n" +"#ifndef B3_PLATFORM_DEFINITIONS_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif\n" +"#endif\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +" typedef float4 b3Quat;\n" +" #define b3QuatConstArg const b3Quat\n" +" \n" +" \n" +"inline float4 b3FastNormalize4(float4 v)\n" +"{\n" +" v = (float4)(v.xyz,0.f);\n" +" return fast_normalize(v);\n" +"}\n" +" \n" +"inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" +"inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" +"inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" +"{\n" +" b3Quat ans;\n" +" ans = b3Cross3( a, b );\n" +" ans += a.w*b+b.w*a;\n" +"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" +" ans.w = a.w*b.w - b3Dot3F4(a, b);\n" +" return ans;\n" +"}\n" +"inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" +"{\n" +" b3Quat q;\n" +" q=in;\n" +" //return b3FastNormalize4(in);\n" +" float len = native_sqrt(dot(q, q));\n" +" if(len > 0.f)\n" +" {\n" +" q *= 1.f / len;\n" +" }\n" +" else\n" +" {\n" +" q.x = q.y = q.z = 0.f;\n" +" q.w = 1.f;\n" +" }\n" +" return q;\n" +"}\n" +"inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" b3Quat qInv = b3QuatInvert( q );\n" +" float4 vcpy = vec;\n" +" vcpy.w = 0.f;\n" +" float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" +" return out;\n" +"}\n" +"inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" +"{\n" +" return (b3Quat)(-q.xyz, q.w);\n" +"}\n" +"inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" +"{\n" +" return b3QuatRotate( b3QuatInvert( q ), vec );\n" +"}\n" +"inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" +"{\n" +" return b3QuatRotate( orientation, point ) + (translation);\n" +"}\n" +" \n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"typedef struct\n" +"{\n" +" b3Float4 m_row[3];\n" +"}b3Mat3x3;\n" +"#define b3Mat3x3ConstArg const b3Mat3x3\n" +"#define b3GetRow(m,row) (m.m_row[row])\n" +"inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" +"{\n" +" b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" +" b3Mat3x3 out;\n" +" out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" +" out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" +" out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" +" out.m_row[0].w = 0.f;\n" +" out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" +" out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" +" out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" +" out.m_row[1].w = 0.f;\n" +" out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" +" out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" +" out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" +" out.m_row[2].w = 0.f;\n" +" return out;\n" +"}\n" +"inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = fabs(matIn.m_row[0]);\n" +" out.m_row[1] = fabs(matIn.m_row[1]);\n" +" out.m_row[2] = fabs(matIn.m_row[2]);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtZero();\n" +"__inline\n" +"b3Mat3x3 mtIdentity();\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" +"__inline\n" +"b3Mat3x3 mtZero()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(0.f);\n" +" m.m_row[1] = (b3Float4)(0.f);\n" +" m.m_row[2] = (b3Float4)(0.f);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtIdentity()\n" +"{\n" +" b3Mat3x3 m;\n" +" m.m_row[0] = (b3Float4)(1,0,0,0);\n" +" m.m_row[1] = (b3Float4)(0,1,0,0);\n" +" m.m_row[2] = (b3Float4)(0,0,1,0);\n" +" return m;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" +"{\n" +" b3Mat3x3 out;\n" +" out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" +" out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" +" out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" +" return out;\n" +"}\n" +"__inline\n" +"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" +"{\n" +" b3Mat3x3 transB;\n" +" transB = mtTranspose( b );\n" +" b3Mat3x3 ans;\n" +" // why this doesn't run when 0ing in the for{}\n" +" a.m_row[0].w = 0.f;\n" +" a.m_row[1].w = 0.f;\n" +" a.m_row[2].w = 0.f;\n" +" for(int i=0; i<3; i++)\n" +" {\n" +"// a.m_row[i].w = 0.f;\n" +" ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" +" ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" +" ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" +" ans.m_row[i].w = 0.f;\n" +" }\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" +"{\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a.m_row[0], b );\n" +" ans.y = b3Dot3F4( a.m_row[1], b );\n" +" ans.z = b3Dot3F4( a.m_row[2], b );\n" +" ans.w = 0.f;\n" +" return ans;\n" +"}\n" +"__inline\n" +"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" +"{\n" +" b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" +" b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" +" b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" +" b3Float4 ans;\n" +" ans.x = b3Dot3F4( a, colx );\n" +" ans.y = b3Dot3F4( a, coly );\n" +" ans.z = b3Dot3F4( a, colz );\n" +" return ans;\n" +"}\n" +"#endif\n" +"#endif //B3_MAT3x3_H\n" +"typedef struct b3Aabb b3Aabb_t;\n" +"struct b3Aabb\n" +"{\n" +" union\n" +" {\n" +" float m_min[4];\n" +" b3Float4 m_minVec;\n" +" int m_minIndices[4];\n" +" };\n" +" union\n" +" {\n" +" float m_max[4];\n" +" b3Float4 m_maxVec;\n" +" int m_signedMaxIndices[4];\n" +" };\n" +"};\n" +"inline void b3TransformAabb2(b3Float4ConstArg localAabbMin,b3Float4ConstArg localAabbMax, float margin,\n" +" b3Float4ConstArg pos,\n" +" b3QuatConstArg orn,\n" +" b3Float4* aabbMinOut,b3Float4* aabbMaxOut)\n" +"{\n" +" b3Float4 localHalfExtents = 0.5f*(localAabbMax-localAabbMin);\n" +" localHalfExtents+=b3MakeFloat4(margin,margin,margin,0.f);\n" +" b3Float4 localCenter = 0.5f*(localAabbMax+localAabbMin);\n" +" b3Mat3x3 m;\n" +" m = b3QuatGetRotationMatrix(orn);\n" +" b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);\n" +" b3Float4 center = b3TransformPoint(localCenter,pos,orn);\n" +" \n" +" b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents,b3GetRow(abs_b,0)),\n" +" b3Dot3F4(localHalfExtents,b3GetRow(abs_b,1)),\n" +" b3Dot3F4(localHalfExtents,b3GetRow(abs_b,2)),\n" +" 0.f);\n" +" *aabbMinOut = center-extent;\n" +" *aabbMaxOut = center+extent;\n" +"}\n" +"/// conservative test for overlap between two aabbs\n" +"inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1,b3Float4ConstArg aabbMax1,\n" +" b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)\n" +"{\n" +" bool overlap = true;\n" +" overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap;\n" +" overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap;\n" +" overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap;\n" +" return overlap;\n" +"}\n" +"#endif //B3_AABB_H\n" +"#ifndef B3_COLLIDABLE_H\n" +"#define B3_COLLIDABLE_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"enum b3ShapeTypes\n" +"{\n" +" SHAPE_HEIGHT_FIELD=1,\n" +" SHAPE_CONVEX_HULL=3,\n" +" SHAPE_PLANE=4,\n" +" SHAPE_CONCAVE_TRIMESH=5,\n" +" SHAPE_COMPOUND_OF_CONVEX_HULLS=6,\n" +" SHAPE_SPHERE=7,\n" +" MAX_NUM_SHAPE_TYPES,\n" +"};\n" +"typedef struct b3Collidable b3Collidable_t;\n" +"struct b3Collidable\n" +"{\n" +" union {\n" +" int m_numChildShapes;\n" +" int m_bvhIndex;\n" +" };\n" +" union\n" +" {\n" +" float m_radius;\n" +" int m_compoundBvhIndex;\n" +" };\n" +" int m_shapeType;\n" +" union\n" +" {\n" +" int m_shapeIndex;\n" +" float m_height;\n" +" };\n" +"};\n" +"typedef struct b3GpuChildShape b3GpuChildShape_t;\n" +"struct b3GpuChildShape\n" +"{\n" +" b3Float4 m_childPosition;\n" +" b3Quat m_childOrientation;\n" +" union\n" +" {\n" +" int m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS\n" +" int m_capsuleAxis;\n" +" };\n" +" union \n" +" {\n" +" float m_radius;//used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES\n" +" int m_numChildShapes;//used for compound shape\n" +" };\n" +" union \n" +" {\n" +" float m_height;//used for childshape of SHAPE_COMPOUND_OF_CAPSULES\n" +" int m_collidableShapeIndex;\n" +" };\n" +" int m_shapeType;\n" +"};\n" +"struct b3CompoundOverlappingPair\n" +"{\n" +" int m_bodyIndexA;\n" +" int m_bodyIndexB;\n" +"// int m_pairType;\n" +" int m_childShapeIndexA;\n" +" int m_childShapeIndexB;\n" +"};\n" +"#endif //B3_COLLIDABLE_H\n" +"#ifndef B3_RIGIDBODY_DATA_H\n" +"#define B3_RIGIDBODY_DATA_H\n" +"#ifndef B3_FLOAT4_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_FLOAT4_H\n" +"#ifndef B3_QUAT_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif \n" +"#endif //B3_QUAT_H\n" +"#ifndef B3_MAT3x3_H\n" +"#ifdef __cplusplus\n" +"#else\n" +"#endif\n" +"#endif //B3_MAT3x3_H\n" +"typedef struct b3RigidBodyData b3RigidBodyData_t;\n" +"struct b3RigidBodyData\n" +"{\n" +" b3Float4 m_pos;\n" +" b3Quat m_quat;\n" +" b3Float4 m_linVel;\n" +" b3Float4 m_angVel;\n" +" int m_collidableIdx;\n" +" float m_invMass;\n" +" float m_restituitionCoeff;\n" +" float m_frictionCoeff;\n" +"};\n" +"typedef struct b3InertiaData b3InertiaData_t;\n" +"struct b3InertiaData\n" +"{\n" +" b3Mat3x3 m_invInertiaWorld;\n" +" b3Mat3x3 m_initInvInertia;\n" +"};\n" +"#endif //B3_RIGIDBODY_DATA_H\n" +" \n" +"void b3ComputeWorldAabb( int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)\n" +"{\n" +" __global const b3RigidBodyData_t* body = &bodies[bodyId];\n" +" b3Float4 position = body->m_pos;\n" +" b3Quat orientation = body->m_quat;\n" +" \n" +" int collidableIndex = body->m_collidableIdx;\n" +" int shapeIndex = collidables[collidableIndex].m_shapeIndex;\n" +" \n" +" if (shapeIndex>=0)\n" +" {\n" +" \n" +" b3Aabb_t localAabb = localShapeAABB[collidableIndex];\n" +" b3Aabb_t worldAabb;\n" +" \n" +" b3Float4 aabbAMinOut,aabbAMaxOut; \n" +" float margin = 0.f;\n" +" b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&aabbAMinOut,&aabbAMaxOut);\n" +" \n" +" worldAabb.m_minVec =aabbAMinOut;\n" +" worldAabb.m_minIndices[3] = bodyId;\n" +" worldAabb.m_maxVec = aabbAMaxOut;\n" +" worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass==0.f? 0 : 1;\n" +" worldAabbs[bodyId] = worldAabb;\n" +" }\n" +"}\n" +"#endif //B3_UPDATE_AABBS_H\n" +"__kernel void initializeGpuAabbsFull( const int numNodes, __global b3RigidBodyData_t* gBodies,__global b3Collidable_t* collidables, __global b3Aabb_t* plocalShapeAABB, __global b3Aabb_t* pAABB)\n" +"{\n" +" int nodeID = get_global_id(0);\n" +" if( nodeID < numNodes )\n" +" {\n" +" b3ComputeWorldAabb(nodeID, gBodies, collidables, plocalShapeAABB,pAABB);\n" +" }\n" +"}\n" +"__kernel void clearOverlappingPairsKernel( __global int4* pairs, int numPairs)\n" +"{\n" +" int pairId = get_global_id(0);\n" +" if( pairId< numPairs )\n" +" {\n" +" pairs[pairId].z = 0xffffffff;\n" +" }\n" +"}\n" +; diff --git a/extern/bullet/src/Bullet3OpenCL/premake4.lua b/extern/bullet/src/Bullet3OpenCL/premake4.lua new file mode 100644 index 000000000000..ee35fdb522ab --- /dev/null +++ b/extern/bullet/src/Bullet3OpenCL/premake4.lua @@ -0,0 +1,32 @@ +function createProject(vendor) + hasCL = findOpenCL(vendor) + + if (hasCL) then + + project ("Bullet3OpenCL_" .. vendor) + + initOpenCL(vendor) + + kind "StaticLib" + + if os.is("Linux") then + buildoptions{"-fPIC"} + end + + includedirs { + ".",".." + } + + files { + "**.cpp", + "**.h" + } + + end +end + +createProject("clew") +createProject("AMD") +createProject("Intel") +createProject("NVIDIA") +createProject("Apple") diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/CMakeLists.txt b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/CMakeLists.txt new file mode 100644 index 000000000000..125576634f09 --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/CMakeLists.txt @@ -0,0 +1,55 @@ + +INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/src +) + +SET(Bullet2FileLoader_SRCS + b3BulletFile.cpp + b3Chunk.cpp + b3DNA.cpp + b3File.cpp + b3Serializer.cpp +) + + +SET(Bullet2FileLoader_HDRS + b3BulletFile.h + b3Chunk.h + b3Common.h + b3Defines.h + b3DNA.h + b3File.h + b3Serializer.h + autogenerated/bullet2.h +) + +ADD_LIBRARY(Bullet2FileLoader ${Bullet2FileLoader_SRCS} ${Bullet2FileLoader_HDRS}) +if (BUILD_SHARED_LIBS) + target_link_libraries(Bullet2FileLoader Bullet3Common) +endif () +SET_TARGET_PROPERTIES(Bullet2FileLoader PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(Bullet2FileLoader PROPERTIES SOVERSION ${BULLET_VERSION}) + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + #FILES_MATCHING requires CMake 2.6 + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS Bullet2FileLoader DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS Bullet2FileLoader + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN +".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(Bullet2FileLoader PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(Bullet2FileLoader PROPERTIES PUBLIC_HEADER "${Bullet2FileLoader_HDRS}") + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h new file mode 100644 index 000000000000..a6b57b1a1280 --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h @@ -0,0 +1,1053 @@ +/* Copyright (C) 2011 Erwin Coumans & Charlie C +* +* 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. +*/ +// Auto generated from Bullet/Extras/HeaderGenerator/bulletGenerate.py +#ifndef __BULLET2_H__ +#define __BULLET2_H__ +namespace Bullet3SerializeBullet2 { + +// put an empty struct in the case +typedef struct bInvalidHandle { + int unused; +}bInvalidHandle; + + class PointerArray; + class b3PhysicsSystem; + class ListBase; + class b3Vector3FloatData; + class b3Vector3DoubleData; + class b3Matrix3x3FloatData; + class b3Matrix3x3DoubleData; + class b3TransformFloatData; + class b3TransformDoubleData; + class b3BvhSubtreeInfoData; + class b3OptimizedBvhNodeFloatData; + class b3OptimizedBvhNodeDoubleData; + class b3QuantizedBvhNodeData; + class b3QuantizedBvhFloatData; + class b3QuantizedBvhDoubleData; + class b3CollisionShapeData; + class b3StaticPlaneShapeData; + class b3ConvexInternalShapeData; + class b3PositionAndRadius; + class b3MultiSphereShapeData; + class b3IntIndexData; + class b3ShortIntIndexData; + class b3ShortIntIndexTripletData; + class b3CharIndexTripletData; + class b3MeshPartData; + class b3StridingMeshInterfaceData; + class b3TriangleMeshShapeData; + class b3ScaledTriangleMeshShapeData; + class b3CompoundShapeChildData; + class b3CompoundShapeData; + class b3CylinderShapeData; + class b3CapsuleShapeData; + class b3TriangleInfoData; + class b3TriangleInfoMapData; + class b3GImpactMeshShapeData; + class b3ConvexHullShapeData; + class b3CollisionObjectDoubleData; + class b3CollisionObjectFloatData; + class b3DynamicsWorldDoubleData; + class b3DynamicsWorldFloatData; + class b3RigidBodyFloatData; + class b3RigidBodyDoubleData; + class b3ConstraintInfo1; + class b3TypedConstraintData; + class b3Point2PointConstraintFloatData; + class b3Point2PointConstraintDoubleData; + class b3HingeConstraintDoubleData; + class b3HingeConstraintFloatData; + class b3ConeTwistConstraintData; + class b3Generic6DofConstraintData; + class b3Generic6DofSpringConstraintData; + class b3SliderConstraintData; + class b3ContactSolverInfoDoubleData; + class b3ContactSolverInfoFloatData; + class SoftBodyMaterialData; + class SoftBodyNodeData; + class SoftBodyLinkData; + class SoftBodyFaceData; + class SoftBodyTetraData; + class SoftRigidAnchorData; + class SoftBodyConfigData; + class SoftBodyPoseData; + class SoftBodyClusterData; + class b3SoftBodyJointData; + class b3SoftBodyFloatData; +// -------------------------------------------------- // + class PointerArray + { + public: + int m_size; + int m_capacity; + void *m_data; + }; + + +// -------------------------------------------------- // + class b3PhysicsSystem + { + public: + PointerArray m_collisionShapes; + PointerArray m_collisionObjects; + PointerArray m_constraints; + }; + + +// -------------------------------------------------- // + class ListBase + { + public: + void *first; + void *last; + }; + + +// -------------------------------------------------- // + class b3Vector3FloatData + { + public: + float m_floats[4]; + }; + + +// -------------------------------------------------- // + class b3Vector3DoubleData + { + public: + double m_floats[4]; + }; + + +// -------------------------------------------------- // + class b3Matrix3x3FloatData + { + public: + b3Vector3FloatData m_el[3]; + }; + + +// -------------------------------------------------- // + class b3Matrix3x3DoubleData + { + public: + b3Vector3DoubleData m_el[3]; + }; + + +// -------------------------------------------------- // + class b3TransformFloatData + { + public: + b3Matrix3x3FloatData m_basis; + b3Vector3FloatData m_origin; + }; + + +// -------------------------------------------------- // + class b3TransformDoubleData + { + public: + b3Matrix3x3DoubleData m_basis; + b3Vector3DoubleData m_origin; + }; + + +// -------------------------------------------------- // + class b3BvhSubtreeInfoData + { + public: + int m_rootNodeIndex; + int m_subtreeSize; + short m_quantizedAabbMin[3]; + short m_quantizedAabbMax[3]; + }; + + +// -------------------------------------------------- // + class b3OptimizedBvhNodeFloatData + { + public: + b3Vector3FloatData m_aabbMinOrg; + b3Vector3FloatData m_aabbMaxOrg; + int m_escapeIndex; + int m_subPart; + int m_triangleIndex; + char m_pad[4]; + }; + + +// -------------------------------------------------- // + class b3OptimizedBvhNodeDoubleData + { + public: + b3Vector3DoubleData m_aabbMinOrg; + b3Vector3DoubleData m_aabbMaxOrg; + int m_escapeIndex; + int m_subPart; + int m_triangleIndex; + char m_pad[4]; + }; + + +// -------------------------------------------------- // + class b3QuantizedBvhNodeData + { + public: + short m_quantizedAabbMin[3]; + short m_quantizedAabbMax[3]; + int m_escapeIndexOrTriangleIndex; + }; + + +// -------------------------------------------------- // + class b3QuantizedBvhFloatData + { + public: + b3Vector3FloatData m_bvhAabbMin; + b3Vector3FloatData m_bvhAabbMax; + b3Vector3FloatData m_bvhQuantization; + int m_curNodeIndex; + int m_useQuantization; + int m_numContiguousLeafNodes; + int m_numQuantizedContiguousNodes; + b3OptimizedBvhNodeFloatData *m_contiguousNodesPtr; + b3QuantizedBvhNodeData *m_quantizedContiguousNodesPtr; + b3BvhSubtreeInfoData *m_subTreeInfoPtr; + int m_traversalMode; + int m_numSubtreeHeaders; + }; + + +// -------------------------------------------------- // + class b3QuantizedBvhDoubleData + { + public: + b3Vector3DoubleData m_bvhAabbMin; + b3Vector3DoubleData m_bvhAabbMax; + b3Vector3DoubleData m_bvhQuantization; + int m_curNodeIndex; + int m_useQuantization; + int m_numContiguousLeafNodes; + int m_numQuantizedContiguousNodes; + b3OptimizedBvhNodeDoubleData *m_contiguousNodesPtr; + b3QuantizedBvhNodeData *m_quantizedContiguousNodesPtr; + int m_traversalMode; + int m_numSubtreeHeaders; + b3BvhSubtreeInfoData *m_subTreeInfoPtr; + }; + + +// -------------------------------------------------- // + class b3CollisionShapeData + { + public: + char *m_name; + int m_shapeType; + char m_padding[4]; + }; + + +// -------------------------------------------------- // + class b3StaticPlaneShapeData + { + public: + b3CollisionShapeData m_collisionShapeData; + b3Vector3FloatData m_localScaling; + b3Vector3FloatData m_planeNormal; + float m_planeConstant; + char m_pad[4]; + }; + + +// -------------------------------------------------- // + class b3ConvexInternalShapeData + { + public: + b3CollisionShapeData m_collisionShapeData; + b3Vector3FloatData m_localScaling; + b3Vector3FloatData m_implicitShapeDimensions; + float m_collisionMargin; + int m_padding; + }; + + +// -------------------------------------------------- // + class b3PositionAndRadius + { + public: + b3Vector3FloatData m_pos; + float m_radius; + }; + + +// -------------------------------------------------- // + class b3MultiSphereShapeData + { + public: + b3ConvexInternalShapeData m_convexInternalShapeData; + b3PositionAndRadius *m_localPositionArrayPtr; + int m_localPositionArraySize; + char m_padding[4]; + }; + + +// -------------------------------------------------- // + class b3IntIndexData + { + public: + int m_value; + }; + + +// -------------------------------------------------- // + class b3ShortIntIndexData + { + public: + short m_value; + char m_pad[2]; + }; + + +// -------------------------------------------------- // + class b3ShortIntIndexTripletData + { + public: + short m_values[3]; + char m_pad[2]; + }; + + +// -------------------------------------------------- // + class b3CharIndexTripletData + { + public: + char m_values[3]; + char m_pad; + }; + + +// -------------------------------------------------- // + class b3MeshPartData + { + public: + b3Vector3FloatData *m_vertices3f; + b3Vector3DoubleData *m_vertices3d; + b3IntIndexData *m_indices32; + b3ShortIntIndexTripletData *m_3indices16; + b3CharIndexTripletData *m_3indices8; + b3ShortIntIndexData *m_indices16; + int m_numTriangles; + int m_numVertices; + }; + + +// -------------------------------------------------- // + class b3StridingMeshInterfaceData + { + public: + b3MeshPartData *m_meshPartsPtr; + b3Vector3FloatData m_scaling; + int m_numMeshParts; + char m_padding[4]; + }; + + +// -------------------------------------------------- // + class b3TriangleMeshShapeData + { + public: + b3CollisionShapeData m_collisionShapeData; + b3StridingMeshInterfaceData m_meshInterface; + b3QuantizedBvhFloatData *m_quantizedFloatBvh; + b3QuantizedBvhDoubleData *m_quantizedDoubleBvh; + b3TriangleInfoMapData *m_triangleInfoMap; + float m_collisionMargin; + char m_pad3[4]; + }; + + +// -------------------------------------------------- // + class b3ScaledTriangleMeshShapeData + { + public: + b3TriangleMeshShapeData m_trimeshShapeData; + b3Vector3FloatData m_localScaling; + }; + + +// -------------------------------------------------- // + class b3CompoundShapeChildData + { + public: + b3TransformFloatData m_transform; + b3CollisionShapeData *m_childShape; + int m_childShapeType; + float m_childMargin; + }; + + +// -------------------------------------------------- // + class b3CompoundShapeData + { + public: + b3CollisionShapeData m_collisionShapeData; + b3CompoundShapeChildData *m_childShapePtr; + int m_numChildShapes; + float m_collisionMargin; + }; + + +// -------------------------------------------------- // + class b3CylinderShapeData + { + public: + b3ConvexInternalShapeData m_convexInternalShapeData; + int m_upAxis; + char m_padding[4]; + }; + + +// -------------------------------------------------- // + class b3CapsuleShapeData + { + public: + b3ConvexInternalShapeData m_convexInternalShapeData; + int m_upAxis; + char m_padding[4]; + }; + + +// -------------------------------------------------- // + class b3TriangleInfoData + { + public: + int m_flags; + float m_edgeV0V1Angle; + float m_edgeV1V2Angle; + float m_edgeV2V0Angle; + }; + + +// -------------------------------------------------- // + class b3TriangleInfoMapData + { + public: + int *m_hashTablePtr; + int *m_nextPtr; + b3TriangleInfoData *m_valueArrayPtr; + int *m_keyArrayPtr; + float m_convexEpsilon; + float m_planarEpsilon; + float m_equalVertexThreshold; + float m_edgeDistanceThreshold; + float m_zeroAreaThreshold; + int m_nextSize; + int m_hashTableSize; + int m_numValues; + int m_numKeys; + char m_padding[4]; + }; + + +// -------------------------------------------------- // + class b3GImpactMeshShapeData + { + public: + b3CollisionShapeData m_collisionShapeData; + b3StridingMeshInterfaceData m_meshInterface; + b3Vector3FloatData m_localScaling; + float m_collisionMargin; + int m_gimpactSubType; + }; + + +// -------------------------------------------------- // + class b3ConvexHullShapeData + { + public: + b3ConvexInternalShapeData m_convexInternalShapeData; + b3Vector3FloatData *m_unscaledPointsFloatPtr; + b3Vector3DoubleData *m_unscaledPointsDoublePtr; + int m_numUnscaledPoints; + char m_padding3[4]; + }; + + +// -------------------------------------------------- // + class b3CollisionObjectDoubleData + { + public: + void *m_broadphaseHandle; + void *m_collisionShape; + b3CollisionShapeData *m_rootCollisionShape; + char *m_name; + b3TransformDoubleData m_worldTransform; + b3TransformDoubleData m_interpolationWorldTransform; + b3Vector3DoubleData m_interpolationLinearVelocity; + b3Vector3DoubleData m_interpolationAngularVelocity; + b3Vector3DoubleData m_anisotropicFriction; + double m_contactProcessingThreshold; + double m_deactivationTime; + double m_friction; + double m_rollingFriction; + double m_restitution; + double m_hitFraction; + double m_ccdSweptSphereRadius; + double m_ccdMotionThreshold; + int m_hasAnisotropicFriction; + int m_collisionFlags; + int m_islandTag1; + int m_companionId; + int m_activationState1; + int m_internalType; + int m_checkCollideWith; + char m_padding[4]; + }; + + +// -------------------------------------------------- // + class b3CollisionObjectFloatData + { + public: + void *m_broadphaseHandle; + void *m_collisionShape; + b3CollisionShapeData *m_rootCollisionShape; + char *m_name; + b3TransformFloatData m_worldTransform; + b3TransformFloatData m_interpolationWorldTransform; + b3Vector3FloatData m_interpolationLinearVelocity; + b3Vector3FloatData m_interpolationAngularVelocity; + b3Vector3FloatData m_anisotropicFriction; + float m_contactProcessingThreshold; + float m_deactivationTime; + float m_friction; + float m_rollingFriction; + float m_restitution; + float m_hitFraction; + float m_ccdSweptSphereRadius; + float m_ccdMotionThreshold; + int m_hasAnisotropicFriction; + int m_collisionFlags; + int m_islandTag1; + int m_companionId; + int m_activationState1; + int m_internalType; + int m_checkCollideWith; + char m_padding[4]; + }; + + + +// -------------------------------------------------- // + class b3RigidBodyFloatData + { + public: + b3CollisionObjectFloatData m_collisionObjectData; + b3Matrix3x3FloatData m_invInertiaTensorWorld; + b3Vector3FloatData m_linearVelocity; + b3Vector3FloatData m_angularVelocity; + b3Vector3FloatData m_angularFactor; + b3Vector3FloatData m_linearFactor; + b3Vector3FloatData m_gravity; + b3Vector3FloatData m_gravity_acceleration; + b3Vector3FloatData m_invInertiaLocal; + b3Vector3FloatData m_totalForce; + b3Vector3FloatData m_totalTorque; + float m_inverseMass; + float m_linearDamping; + float m_angularDamping; + float m_additionalDampingFactor; + float m_additionalLinearDampingThresholdSqr; + float m_additionalAngularDampingThresholdSqr; + float m_additionalAngularDampingFactor; + float m_linearSleepingThreshold; + float m_angularSleepingThreshold; + int m_additionalDamping; + }; + + +// -------------------------------------------------- // + class b3RigidBodyDoubleData + { + public: + b3CollisionObjectDoubleData m_collisionObjectData; + b3Matrix3x3DoubleData m_invInertiaTensorWorld; + b3Vector3DoubleData m_linearVelocity; + b3Vector3DoubleData m_angularVelocity; + b3Vector3DoubleData m_angularFactor; + b3Vector3DoubleData m_linearFactor; + b3Vector3DoubleData m_gravity; + b3Vector3DoubleData m_gravity_acceleration; + b3Vector3DoubleData m_invInertiaLocal; + b3Vector3DoubleData m_totalForce; + b3Vector3DoubleData m_totalTorque; + double m_inverseMass; + double m_linearDamping; + double m_angularDamping; + double m_additionalDampingFactor; + double m_additionalLinearDampingThresholdSqr; + double m_additionalAngularDampingThresholdSqr; + double m_additionalAngularDampingFactor; + double m_linearSleepingThreshold; + double m_angularSleepingThreshold; + int m_additionalDamping; + char m_padding[4]; + }; + + +// -------------------------------------------------- // + class b3ConstraintInfo1 + { + public: + int m_numConstraintRows; + int nub; + }; + + +// -------------------------------------------------- // + class b3TypedConstraintData + { + public: + bInvalidHandle *m_rbA; + bInvalidHandle *m_rbB; + char *m_name; + int m_objectType; + int m_userConstraintType; + int m_userConstraintId; + int m_needsFeedback; + float m_appliedImpulse; + float m_dbgDrawSize; + int m_disableCollisionsBetweenLinkedBodies; + int m_overrideNumSolverIterations; + float m_breakingImpulseThreshold; + int m_isEnabled; + }; + + +// -------------------------------------------------- // + class b3Point2PointConstraintFloatData + { + public: + b3TypedConstraintData m_typeConstraintData; + b3Vector3FloatData m_pivotInA; + b3Vector3FloatData m_pivotInB; + }; + + +// -------------------------------------------------- // + class b3Point2PointConstraintDoubleData + { + public: + b3TypedConstraintData m_typeConstraintData; + b3Vector3DoubleData m_pivotInA; + b3Vector3DoubleData m_pivotInB; + }; + + +// -------------------------------------------------- // + class b3HingeConstraintDoubleData + { + public: + b3TypedConstraintData m_typeConstraintData; + b3TransformDoubleData m_rbAFrame; + b3TransformDoubleData m_rbBFrame; + int m_useReferenceFrameA; + int m_angularOnly; + int m_enableAngularMotor; + float m_motorTargetVelocity; + float m_maxMotorImpulse; + float m_lowerLimit; + float m_upperLimit; + float m_limitSoftness; + float m_biasFactor; + float m_relaxationFactor; + }; + + +// -------------------------------------------------- // + class b3HingeConstraintFloatData + { + public: + b3TypedConstraintData m_typeConstraintData; + b3TransformFloatData m_rbAFrame; + b3TransformFloatData m_rbBFrame; + int m_useReferenceFrameA; + int m_angularOnly; + int m_enableAngularMotor; + float m_motorTargetVelocity; + float m_maxMotorImpulse; + float m_lowerLimit; + float m_upperLimit; + float m_limitSoftness; + float m_biasFactor; + float m_relaxationFactor; + }; + + +// -------------------------------------------------- // + class b3ConeTwistConstraintData + { + public: + b3TypedConstraintData m_typeConstraintData; + b3TransformFloatData m_rbAFrame; + b3TransformFloatData m_rbBFrame; + float m_swingSpan1; + float m_swingSpan2; + float m_twistSpan; + float m_limitSoftness; + float m_biasFactor; + float m_relaxationFactor; + float m_damping; + char m_pad[4]; + }; + + +// -------------------------------------------------- // + class b3Generic6DofConstraintData + { + public: + b3TypedConstraintData m_typeConstraintData; + b3TransformFloatData m_rbAFrame; + b3TransformFloatData m_rbBFrame; + b3Vector3FloatData m_linearUpperLimit; + b3Vector3FloatData m_linearLowerLimit; + b3Vector3FloatData m_angularUpperLimit; + b3Vector3FloatData m_angularLowerLimit; + int m_useLinearReferenceFrameA; + int m_useOffsetForConstraintFrame; + }; + + +// -------------------------------------------------- // + class b3Generic6DofSpringConstraintData + { + public: + b3Generic6DofConstraintData m_6dofData; + int m_springEnabled[6]; + float m_equilibriumPoint[6]; + float m_springStiffness[6]; + float m_springDamping[6]; + }; + + +// -------------------------------------------------- // + class b3SliderConstraintData + { + public: + b3TypedConstraintData m_typeConstraintData; + b3TransformFloatData m_rbAFrame; + b3TransformFloatData m_rbBFrame; + float m_linearUpperLimit; + float m_linearLowerLimit; + float m_angularUpperLimit; + float m_angularLowerLimit; + int m_useLinearReferenceFrameA; + int m_useOffsetForConstraintFrame; + }; + + +// -------------------------------------------------- // + class b3ContactSolverInfoDoubleData + { + public: + double m_tau; + double m_damping; + double m_friction; + double m_timeStep; + double m_restitution; + double m_maxErrorReduction; + double m_sor; + double m_erp; + double m_erp2; + double m_globalCfm; + double m_splitImpulsePenetrationThreshold; + double m_splitImpulseTurnErp; + double m_linearSlop; + double m_warmstartingFactor; + double m_maxGyroscopicForce; + double m_singleAxisRollingFrictionThreshold; + int m_numIterations; + int m_solverMode; + int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; + int m_splitImpulse; + char m_padding[4]; + }; + + +// -------------------------------------------------- // + class b3ContactSolverInfoFloatData + { + public: + float m_tau; + float m_damping; + float m_friction; + float m_timeStep; + float m_restitution; + float m_maxErrorReduction; + float m_sor; + float m_erp; + float m_erp2; + float m_globalCfm; + float m_splitImpulsePenetrationThreshold; + float m_splitImpulseTurnErp; + float m_linearSlop; + float m_warmstartingFactor; + float m_maxGyroscopicForce; + float m_singleAxisRollingFrictionThreshold; + int m_numIterations; + int m_solverMode; + int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; + int m_splitImpulse; + char m_padding[4]; + }; + + + // -------------------------------------------------- // + class b3DynamicsWorldDoubleData + { + public: + b3ContactSolverInfoDoubleData m_solverInfo; + b3Vector3DoubleData m_gravity; + }; + + +// -------------------------------------------------- // + class b3DynamicsWorldFloatData + { + public: + b3ContactSolverInfoFloatData m_solverInfo; + b3Vector3FloatData m_gravity; + }; + + + +// -------------------------------------------------- // + class SoftBodyMaterialData + { + public: + float m_linearStiffness; + float m_angularStiffness; + float m_volumeStiffness; + int m_flags; + }; + + +// -------------------------------------------------- // + class SoftBodyNodeData + { + public: + SoftBodyMaterialData *m_material; + b3Vector3FloatData m_position; + b3Vector3FloatData m_previousPosition; + b3Vector3FloatData m_velocity; + b3Vector3FloatData m_accumulatedForce; + b3Vector3FloatData m_normal; + float m_inverseMass; + float m_area; + int m_attach; + int m_pad; + }; + + +// -------------------------------------------------- // + class SoftBodyLinkData + { + public: + SoftBodyMaterialData *m_material; + int m_nodeIndices[2]; + float m_restLength; + int m_bbending; + }; + + +// -------------------------------------------------- // + class SoftBodyFaceData + { + public: + b3Vector3FloatData m_normal; + SoftBodyMaterialData *m_material; + int m_nodeIndices[3]; + float m_restArea; + }; + + +// -------------------------------------------------- // + class SoftBodyTetraData + { + public: + b3Vector3FloatData m_c0[4]; + SoftBodyMaterialData *m_material; + int m_nodeIndices[4]; + float m_restVolume; + float m_c1; + float m_c2; + int m_pad; + }; + + +// -------------------------------------------------- // + class SoftRigidAnchorData + { + public: + b3Matrix3x3FloatData m_c0; + b3Vector3FloatData m_c1; + b3Vector3FloatData m_localFrame; + bInvalidHandle *m_rigidBody; + int m_nodeIndex; + float m_c2; + }; + + +// -------------------------------------------------- // + class SoftBodyConfigData + { + public: + int m_aeroModel; + float m_baumgarte; + float m_damping; + float m_drag; + float m_lift; + float m_pressure; + float m_volume; + float m_dynamicFriction; + float m_poseMatch; + float m_rigidContactHardness; + float m_kineticContactHardness; + float m_softContactHardness; + float m_anchorHardness; + float m_softRigidClusterHardness; + float m_softKineticClusterHardness; + float m_softSoftClusterHardness; + float m_softRigidClusterImpulseSplit; + float m_softKineticClusterImpulseSplit; + float m_softSoftClusterImpulseSplit; + float m_maxVolume; + float m_timeScale; + int m_velocityIterations; + int m_positionIterations; + int m_driftIterations; + int m_clusterIterations; + int m_collisionFlags; + }; + + +// -------------------------------------------------- // + class SoftBodyPoseData + { + public: + b3Matrix3x3FloatData m_rot; + b3Matrix3x3FloatData m_scale; + b3Matrix3x3FloatData m_aqq; + b3Vector3FloatData m_com; + b3Vector3FloatData *m_positions; + float *m_weights; + int m_numPositions; + int m_numWeigts; + int m_bvolume; + int m_bframe; + float m_restVolume; + int m_pad; + }; + + +// -------------------------------------------------- // + class SoftBodyClusterData + { + public: + b3TransformFloatData m_framexform; + b3Matrix3x3FloatData m_locii; + b3Matrix3x3FloatData m_invwi; + b3Vector3FloatData m_com; + b3Vector3FloatData m_vimpulses[2]; + b3Vector3FloatData m_dimpulses[2]; + b3Vector3FloatData m_lv; + b3Vector3FloatData m_av; + b3Vector3FloatData *m_framerefs; + int *m_nodeIndices; + float *m_masses; + int m_numFrameRefs; + int m_numNodes; + int m_numMasses; + float m_idmass; + float m_imass; + int m_nvimpulses; + int m_ndimpulses; + float m_ndamping; + float m_ldamping; + float m_adamping; + float m_matching; + float m_maxSelfCollisionImpulse; + float m_selfCollisionImpulseFactor; + int m_containsAnchor; + int m_collide; + int m_clusterIndex; + }; + + +// -------------------------------------------------- // + class b3SoftBodyJointData + { + public: + void *m_bodyA; + void *m_bodyB; + b3Vector3FloatData m_refs[2]; + float m_cfm; + float m_erp; + float m_split; + int m_delete; + b3Vector3FloatData m_relPosition[2]; + int m_bodyAtype; + int m_bodyBtype; + int m_jointType; + int m_pad; + }; + + +// -------------------------------------------------- // + class b3SoftBodyFloatData + { + public: + b3CollisionObjectFloatData m_collisionObjectData; + SoftBodyPoseData *m_pose; + SoftBodyMaterialData **m_materials; + SoftBodyNodeData *m_nodes; + SoftBodyLinkData *m_links; + SoftBodyFaceData *m_faces; + SoftBodyTetraData *m_tetrahedra; + SoftRigidAnchorData *m_anchors; + SoftBodyClusterData *m_clusters; + b3SoftBodyJointData *m_joints; + int m_numMaterials; + int m_numNodes; + int m_numLinks; + int m_numFaces; + int m_numTetrahedra; + int m_numAnchors; + int m_numClusters; + int m_numJoints; + SoftBodyConfigData m_config; + }; + + +} +#endif//__BULLET2_H__ \ No newline at end of file diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.cpp b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.cpp new file mode 100644 index 000000000000..c3ceb8388ca3 --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.cpp @@ -0,0 +1,423 @@ +/* +bParse +Copyright (c) 2006-2010 Erwin Coumans http://gamekit.googlecode.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 "b3BulletFile.h" +#include "b3Defines.h" +#include "b3DNA.h" + +#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) +#include +#endif +#include + + +// 32 && 64 bit versions +#ifdef B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES +#ifdef _WIN64 +extern char b3s_bulletDNAstr64[]; +extern int b3s_bulletDNAlen64; +#else +extern char b3s_bulletDNAstr[]; +extern int b3s_bulletDNAlen; +#endif //_WIN64 +#else//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + +extern char b3s_bulletDNAstr64[]; +extern int b3s_bulletDNAlen64; +extern char b3s_bulletDNAstr[]; +extern int b3s_bulletDNAlen; + +#endif //B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + +using namespace bParse; + +b3BulletFile::b3BulletFile() +:bFile("", "BULLET ") +{ + mMemoryDNA = new bDNA(); //this memory gets released in the bFile::~bFile destructor,@todo not consistent with the rule 'who allocates it, has to deallocate it" + + m_DnaCopy = 0; + + +#ifdef B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES +#ifdef _WIN64 + m_DnaCopy = (char*)b3AlignedAlloc(b3s_bulletDNAlen64,16); + memcpy(m_DnaCopy,b3s_bulletDNAstr64,b3s_bulletDNAlen64); + mMemoryDNA->init(m_DnaCopy,b3s_bulletDNAlen64); +#else//_WIN64 + m_DnaCopy = (char*)b3AlignedAlloc(b3s_bulletDNAlen,16); + memcpy(m_DnaCopy,b3s_bulletDNAstr,b3s_bulletDNAlen); + mMemoryDNA->init(m_DnaCopy,b3s_bulletDNAlen); +#endif//_WIN64 +#else//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + if (VOID_IS_8) + { + m_DnaCopy = (char*) b3AlignedAlloc(b3s_bulletDNAlen64,16); + memcpy(m_DnaCopy,b3s_bulletDNAstr64,b3s_bulletDNAlen64); + mMemoryDNA->init(m_DnaCopy,b3s_bulletDNAlen64); + } + else + { + m_DnaCopy =(char*) b3AlignedAlloc(b3s_bulletDNAlen,16); + memcpy(m_DnaCopy,b3s_bulletDNAstr,b3s_bulletDNAlen); + mMemoryDNA->init(m_DnaCopy,b3s_bulletDNAlen); + } +#endif//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES +} + + + +b3BulletFile::b3BulletFile(const char* fileName) +:bFile(fileName, "BULLET ") +{ + m_DnaCopy = 0; +} + + + +b3BulletFile::b3BulletFile(char *memoryBuffer, int len) +:bFile(memoryBuffer,len, "BULLET ") +{ + m_DnaCopy = 0; +} + + +b3BulletFile::~b3BulletFile() +{ + if (m_DnaCopy) + b3AlignedFree(m_DnaCopy); + + + while (m_dataBlocks.size()) + { + char* dataBlock = m_dataBlocks[m_dataBlocks.size()-1]; + delete[] dataBlock; + m_dataBlocks.pop_back(); + } + +} + + + +// ----------------------------------------------------- // +void b3BulletFile::parseData() +{ +// printf ("Building datablocks"); +// printf ("Chunk size = %d",CHUNK_HEADER_LEN); +// printf ("File chunk size = %d",ChunkUtils::getOffset(mFlags)); + + const bool brokenDNA = (mFlags&FD_BROKEN_DNA)!=0; + + //const bool swap = (mFlags&FD_ENDIAN_SWAP)!=0; + + + mDataStart = 12; + + char *dataPtr = mFileBuffer+mDataStart; + + bChunkInd dataChunk; + dataChunk.code = 0; + + + //dataPtr += ChunkUtils::getNextBlock(&dataChunk, dataPtr, mFlags); + int seek = getNextBlock(&dataChunk, dataPtr, mFlags); + + + if (mFlags &FD_ENDIAN_SWAP) + swapLen(dataPtr); + + //dataPtr += ChunkUtils::getOffset(mFlags); + char *dataPtrHead = 0; + + while (dataChunk.code != B3_DNA1) + { + if (!brokenDNA || (dataChunk.code != B3_QUANTIZED_BVH_CODE) ) + { + + // one behind + if (dataChunk.code == B3_SDNA) break; + //if (dataChunk.code == DNA1) break; + + // same as (BHEAD+DATA dependency) + dataPtrHead = dataPtr+ChunkUtils::getOffset(mFlags); + if (dataChunk.dna_nr>=0) + { + char *id = readStruct(dataPtrHead, dataChunk); + + // lookup maps + if (id) + { + m_chunkPtrPtrMap.insert(dataChunk.oldPtr, dataChunk); + mLibPointers.insert(dataChunk.oldPtr, (bStructHandle*)id); + + m_chunks.push_back(dataChunk); + // block it + //bListBasePtr *listID = mMain->getListBasePtr(dataChunk.code); + //if (listID) + // listID->push_back((bStructHandle*)id); + } + + if (dataChunk.code == B3_SOFTBODY_CODE) + { + m_softBodies.push_back((bStructHandle*) id); + } + + if (dataChunk.code == B3_RIGIDBODY_CODE) + { + m_rigidBodies.push_back((bStructHandle*) id); + } + + if (dataChunk.code == B3_DYNAMICSWORLD_CODE) + { + m_dynamicsWorldInfo.push_back((bStructHandle*) id); + } + + if (dataChunk.code == B3_CONSTRAINT_CODE) + { + m_constraints.push_back((bStructHandle*) id); + } + + if (dataChunk.code == B3_QUANTIZED_BVH_CODE) + { + m_bvhs.push_back((bStructHandle*) id); + } + + if (dataChunk.code == B3_TRIANLGE_INFO_MAP) + { + m_triangleInfoMaps.push_back((bStructHandle*) id); + } + + if (dataChunk.code == B3_COLLISIONOBJECT_CODE) + { + m_collisionObjects.push_back((bStructHandle*) id); + } + + if (dataChunk.code == B3_SHAPE_CODE) + { + m_collisionShapes.push_back((bStructHandle*) id); + } + + // if (dataChunk.code == GLOB) + // { + // m_glob = (bStructHandle*) id; + // } + } else + { + //printf("unknown chunk\n"); + + mLibPointers.insert(dataChunk.oldPtr, (bStructHandle*)dataPtrHead); + } + } else + { + printf("skipping B3_QUANTIZED_BVH_CODE due to broken DNA\n"); + } + + + dataPtr += seek; + + seek = getNextBlock(&dataChunk, dataPtr, mFlags); + if (mFlags &FD_ENDIAN_SWAP) + swapLen(dataPtr); + + if (seek < 0) + break; + } + +} + +void b3BulletFile::addDataBlock(char* dataBlock) +{ + m_dataBlocks.push_back(dataBlock); + +} + + + + +void b3BulletFile::writeDNA(FILE* fp) +{ + + bChunkInd dataChunk; + dataChunk.code = B3_DNA1; + dataChunk.dna_nr = 0; + dataChunk.nr = 1; +#ifdef B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + if (VOID_IS_8) + { +#ifdef _WIN64 + dataChunk.len = b3s_bulletDNAlen64; + dataChunk.oldPtr = b3s_bulletDNAstr64; + fwrite(&dataChunk,sizeof(bChunkInd),1,fp); + fwrite(b3s_bulletDNAstr64, b3s_bulletDNAlen64,1,fp); +#else + b3Assert(0); +#endif + } + else + { +#ifndef _WIN64 + dataChunk.len = b3s_bulletDNAlen; + dataChunk.oldPtr = b3s_bulletDNAstr; + fwrite(&dataChunk,sizeof(bChunkInd),1,fp); + fwrite(b3s_bulletDNAstr, b3s_bulletDNAlen,1,fp); +#else//_WIN64 + b3Assert(0); +#endif//_WIN64 + } +#else//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + if (VOID_IS_8) + { + dataChunk.len = b3s_bulletDNAlen64; + dataChunk.oldPtr = b3s_bulletDNAstr64; + fwrite(&dataChunk,sizeof(bChunkInd),1,fp); + fwrite(b3s_bulletDNAstr64, b3s_bulletDNAlen64,1,fp); + } + else + { + dataChunk.len = b3s_bulletDNAlen; + dataChunk.oldPtr = b3s_bulletDNAstr; + fwrite(&dataChunk,sizeof(bChunkInd),1,fp); + fwrite(b3s_bulletDNAstr, b3s_bulletDNAlen,1,fp); + } +#endif//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES +} + + +void b3BulletFile::parse(int verboseMode) +{ +#ifdef B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + if (VOID_IS_8) + { +#ifdef _WIN64 + + if (m_DnaCopy) + delete m_DnaCopy; + m_DnaCopy = (char*)b3AlignedAlloc(b3s_bulletDNAlen64,16); + memcpy(m_DnaCopy,b3s_bulletDNAstr64,b3s_bulletDNAlen64); + parseInternal(verboseMode,(char*)b3s_bulletDNAstr64,b3s_bulletDNAlen64); +#else + b3Assert(0); +#endif + } + else + { +#ifndef _WIN64 + + if (m_DnaCopy) + delete m_DnaCopy; + m_DnaCopy = (char*)b3AlignedAlloc(b3s_bulletDNAlen,16); + memcpy(m_DnaCopy,b3s_bulletDNAstr,b3s_bulletDNAlen); + parseInternal(verboseMode,m_DnaCopy,b3s_bulletDNAlen); +#else + b3Assert(0); +#endif + } +#else//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + if (VOID_IS_8) + { + if (m_DnaCopy) + delete m_DnaCopy; + m_DnaCopy = (char*)b3AlignedAlloc(b3s_bulletDNAlen64,16); + memcpy(m_DnaCopy,b3s_bulletDNAstr64,b3s_bulletDNAlen64); + parseInternal(verboseMode,m_DnaCopy,b3s_bulletDNAlen64); + } + else + { + if (m_DnaCopy) + delete m_DnaCopy; + m_DnaCopy = (char*)b3AlignedAlloc(b3s_bulletDNAlen,16); + memcpy(m_DnaCopy,b3s_bulletDNAstr,b3s_bulletDNAlen); + parseInternal(verboseMode,m_DnaCopy,b3s_bulletDNAlen); + } +#endif//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES + + //the parsing will convert to cpu endian + mFlags &=~FD_ENDIAN_SWAP; + + int littleEndian= 1; + littleEndian= ((char*)&littleEndian)[0]; + + mFileBuffer[8] = littleEndian?'v':'V'; + +} + +// experimental +int b3BulletFile::write(const char* fileName, bool fixupPointers) +{ + FILE *fp = fopen(fileName, "wb"); + if (fp) + { + char header[B3_SIZEOFBLENDERHEADER] ; + memcpy(header, m_headerString, 7); + int endian= 1; + endian= ((char*)&endian)[0]; + + if (endian) + { + header[7] = '_'; + } else + { + header[7] = '-'; + } + if (VOID_IS_8) + { + header[8]='V'; + } else + { + header[8]='v'; + } + + header[9] = '2'; + header[10] = '7'; + header[11] = '5'; + + fwrite(header,B3_SIZEOFBLENDERHEADER,1,fp); + + writeChunks(fp, fixupPointers); + + writeDNA(fp); + + fclose(fp); + + } else + { + printf("Error: cannot open file %s for writing\n",fileName); + return 0; + } + return 1; +} + + + +void b3BulletFile::addStruct(const char* structType,void* data, int len, void* oldPtr, int code) +{ + + bParse::bChunkInd dataChunk; + dataChunk.code = code; + dataChunk.nr = 1; + dataChunk.len = len; + dataChunk.dna_nr = mMemoryDNA->getReverseType(structType); + dataChunk.oldPtr = oldPtr; + + ///Perform structure size validation + short* structInfo= mMemoryDNA->getStruct(dataChunk.dna_nr); + int elemBytes; + elemBytes= mMemoryDNA->getLength(structInfo[0]); +// int elemBytes = mMemoryDNA->getElementSize(structInfo[0],structInfo[1]); + assert(len==elemBytes); + + mLibPointers.insert(dataChunk.oldPtr, (bStructHandle*)data); + m_chunks.push_back(dataChunk); +} diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.h b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.h new file mode 100644 index 000000000000..fb1b9b0dde4c --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.h @@ -0,0 +1,83 @@ +/* +bParse +Copyright (c) 2006-2010 Charlie C & Erwin Coumans http://gamekit.googlecode.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_BULLET_FILE_H +#define B3_BULLET_FILE_H + + +#include "b3File.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "b3Defines.h" + +#include "Bullet3Serialize/Bullet2FileLoader/b3Serializer.h" + + + +namespace bParse { + + // ----------------------------------------------------- // + class b3BulletFile : public bFile + { + + + protected: + + char* m_DnaCopy; + + public: + + b3AlignedObjectArray m_softBodies; + + b3AlignedObjectArray m_rigidBodies; + + b3AlignedObjectArray m_collisionObjects; + + b3AlignedObjectArray m_collisionShapes; + + b3AlignedObjectArray m_constraints; + + b3AlignedObjectArray m_bvhs; + + b3AlignedObjectArray m_triangleInfoMaps; + + b3AlignedObjectArray m_dynamicsWorldInfo; + + b3AlignedObjectArray m_dataBlocks; + b3BulletFile(); + + b3BulletFile(const char* fileName); + + b3BulletFile(char *memoryBuffer, int len); + + virtual ~b3BulletFile(); + + virtual void addDataBlock(char* dataBlock); + + + // experimental + virtual int write(const char* fileName, bool fixupPointers=false); + + virtual void parse(int verboseMode); + + virtual void parseData(); + + virtual void writeDNA(FILE* fp); + + void addStruct(const char* structType,void* data, int len, void* oldPtr, int code); + + }; +}; + +#endif //B3_BULLET_FILE_H diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.cpp b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.cpp new file mode 100644 index 000000000000..c0e1bb708cea --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.cpp @@ -0,0 +1,75 @@ +/* +bParse +Copyright (c) 2006-2009 Charlie C & Erwin Coumans http://gamekit.googlecode.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 "b3Chunk.h" +#include "b3Defines.h" +#include "b3File.h" + +#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) +#include +#endif +#include + + +using namespace bParse; + + +// ----------------------------------------------------- // +short ChunkUtils::swapShort(short sht) +{ + B3_SWITCH_SHORT(sht); + return sht; +} + +// ----------------------------------------------------- // +int ChunkUtils::swapInt(int inte) +{ + B3_SWITCH_INT(inte); + return inte; +} + +// ----------------------------------------------------- // +b3Long64 ChunkUtils::swapLong64(b3Long64 lng) +{ + B3_SWITCH_LONGINT(lng); + return lng; +} + +// ----------------------------------------------------- // +int ChunkUtils::getOffset(int flags) +{ + // if the file is saved in a + // different format, get the + // file's chunk size + int res = CHUNK_HEADER_LEN; + + if (VOID_IS_8) + { + if (flags &FD_BITS_VARIES) + res = sizeof(bChunkPtr4); + } + else + { + if (flags &FD_BITS_VARIES) + res = sizeof(bChunkPtr8); + } + return res; +} + + + + + +//eof diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.h b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.h new file mode 100644 index 000000000000..03ecb6b4faf8 --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Chunk.h @@ -0,0 +1,92 @@ +/* +bParse +Copyright (c) 2006-2009 Charlie C & Erwin Coumans http://gamekit.googlecode.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef __BCHUNK_H__ +#define __BCHUNK_H__ + +#if defined (_WIN32) && ! defined (__MINGW32__) + #define b3Long64 __int64 +#elif defined (__MINGW32__) + #include + #define b3Long64 int64_t +#else + #define b3Long64 long long +#endif + + +namespace bParse { + + + // ----------------------------------------------------- // + class bChunkPtr4 + { + public: + bChunkPtr4(){} + int code; + int len; + union + { + int m_uniqueInt; + }; + int dna_nr; + int nr; + }; + + // ----------------------------------------------------- // + class bChunkPtr8 + { + public: + bChunkPtr8(){} + int code, len; + union + { + b3Long64 oldPrev; + int m_uniqueInts[2]; + }; + int dna_nr, nr; + }; + + // ----------------------------------------------------- // + class bChunkInd + { + public: + bChunkInd(){} + int code, len; + void *oldPtr; + int dna_nr, nr; + }; + + + // ----------------------------------------------------- // + class ChunkUtils + { + public: + + // file chunk offset + static int getOffset(int flags); + + // endian utils + static short swapShort(short sht); + static int swapInt(int inte); + static b3Long64 swapLong64(b3Long64 lng); + + }; + + + const int CHUNK_HEADER_LEN = ((sizeof(bChunkInd))); + const bool VOID_IS_8 = ((sizeof(void*)==8)); +} + +#endif//__BCHUNK_H__ diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Common.h b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Common.h new file mode 100644 index 000000000000..2792d84033f1 --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Common.h @@ -0,0 +1,39 @@ +/* +bParse +Copyright (c) 2006-2009 Charlie C & Erwin Coumans http://gamekit.googlecode.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef __BCOMMON_H__ +#define __BCOMMON_H__ + + +#include +//#include "bLog.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3HashMap.h" + +namespace bParse { + + class bMain; + class bFileData; + class bFile; + class bDNA; + + // delete void* undefined + typedef struct bStructHandle {int unused;}bStructHandle; + typedef b3AlignedObjectArray bListBasePtr; + typedef b3HashMap bPtrMap; +} + + +#endif//__BCOMMON_H__ diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp new file mode 100644 index 000000000000..0fe50569221d --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp @@ -0,0 +1,629 @@ +/* +bParse +Copyright (c) 2006-2009 Charlie C & Erwin Coumans http://gamekit.googlecode.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 + +#include "b3DNA.h" +#include "b3Chunk.h" +#include +#include +#include + +//this define will force traversal of structures, to check backward (and forward) compatibility +//#define TEST_BACKWARD_FORWARD_COMPATIBILITY + + +using namespace bParse; + + +// ----------------------------------------------------- // +bDNA::bDNA() + : mPtrLen(0) +{ + // -- +} + +// ----------------------------------------------------- // +bDNA::~bDNA() +{ + // -- +} + +// ----------------------------------------------------- // +bool bDNA::lessThan(bDNA *file) +{ + return ( m_Names.size() < file->m_Names.size()); +} + +// ----------------------------------------------------- // +char *bDNA::getName(int ind) +{ + assert(ind <= (int)m_Names.size()); + return m_Names[ind].m_name; +} + + +// ----------------------------------------------------- // +char *bDNA::getType(int ind) +{ + assert(ind<= (int)mTypes.size()); + return mTypes[ind]; +} + + +// ----------------------------------------------------- // +short *bDNA::getStruct(int ind) +{ + assert(ind <= (int)mStructs.size()); + return mStructs[ind]; +} + + +// ----------------------------------------------------- // +short bDNA::getLength(int ind) +{ + assert(ind <= (int)mTlens.size()); + return mTlens[ind]; +} + + +// ----------------------------------------------------- // +int bDNA::getReverseType(short type) +{ + + int* intPtr = mStructReverse.find(type); + if (intPtr) + return *intPtr; + + return -1; +} + +// ----------------------------------------------------- // +int bDNA::getReverseType(const char *type) +{ + + b3HashString key(type); + int* valuePtr = mTypeLookup.find(key); + if (valuePtr) + return *valuePtr; + + return -1; +} + +// ----------------------------------------------------- // +int bDNA::getNumStructs() +{ + return (int)mStructs.size(); +} + +// ----------------------------------------------------- // +bool bDNA::flagNotEqual(int dna_nr) +{ + assert(dna_nr <= (int)mCMPFlags.size()); + return mCMPFlags[dna_nr] == FDF_STRUCT_NEQU; +} + +// ----------------------------------------------------- // +bool bDNA::flagEqual(int dna_nr) +{ + assert(dna_nr <= (int)mCMPFlags.size()); + int flag = mCMPFlags[dna_nr]; + return flag == FDF_STRUCT_EQU; +} + +// ----------------------------------------------------- // +bool bDNA::flagNone(int dna_nr) +{ + assert(dna_nr <= (int)mCMPFlags.size()); + return mCMPFlags[dna_nr] == FDF_NONE; +} + +// ----------------------------------------------------- // +int bDNA::getPointerSize() +{ + return mPtrLen; +} + +// ----------------------------------------------------- // +void bDNA::initRecurseCmpFlags(int iter) +{ + // iter is FDF_STRUCT_NEQU + + short *oldStrc = mStructs[iter]; + short type = oldStrc[0]; + + for (int i=0; i<(int)mStructs.size(); i++) + { + if (i != iter && mCMPFlags[i] == FDF_STRUCT_EQU ) + { + short *curStruct = mStructs[i]; + int eleLen = curStruct[1]; + curStruct+=2; + + for (int j=0; jgetReverseType(typeName); + if (newLookup == -1) + { + mCMPFlags[i] = FDF_NONE; + continue; + } + short *curStruct = memDNA->mStructs[newLookup]; +#else + // memory for file + + if (oldLookup < memDNA->mStructs.size()) + { + short *curStruct = memDNA->mStructs[oldLookup]; +#endif + + + + // rebuild... + mCMPFlags[i] = FDF_STRUCT_NEQU; + +#ifndef TEST_BACKWARD_FORWARD_COMPATIBILITY + + if (curStruct[1] == oldStruct[1]) + { + // type len same ... + if (mTlens[oldStruct[0]] == memDNA->mTlens[curStruct[0]]) + { + bool isSame = true; + int elementLength = oldStruct[1]; + + + curStruct+=2; + oldStruct+=2; + + + for (int j=0; jmTypes[curStruct[0]])!=0) + { + isSame=false; + break; + } + + // name the same + if (strcmp(m_Names[oldStruct[1]].m_name, memDNA->m_Names[curStruct[1]].m_name)!=0) + { + isSame=false; + break; + } + } + // flag valid == + if (isSame) + mCMPFlags[i] = FDF_STRUCT_EQU; + } + } +#endif + } + } + + + + + + // recurse in + for ( i=0; i<(int)mStructs.size(); i++) + { + if (mCMPFlags[i] == FDF_STRUCT_NEQU) + initRecurseCmpFlags(i); + } +} + + + + +static int name_is_array(char* name, int* dim1, int* dim2) { + int len = strlen(name); + /*fprintf(stderr,"[%s]",name);*/ + /*if (len >= 1) { + if (name[len-1] != ']') + return 1; + } + return 0;*/ + char *bp; + int num; + if (dim1) { + *dim1 = 1; + } + if (dim2) { + *dim2 = 1; + } + bp = strchr(name, '['); + if (!bp) { + return 0; + } + num = 0; + while (++bp < name+len-1) { + const char c = *bp; + if (c == ']') { + break; + } + if (c <= '9' && c >= '0') { + num *= 10; + num += (c - '0'); + } else { + printf("array parse error.\n"); + return 0; + } + } + if (dim2) { + *dim2 = num; + } + + /* find second dim, if any. */ + bp = strchr(bp, '['); + if (!bp) { + return 1; /* at least we got the first dim. */ + } + num = 0; + while (++bp < name+len-1) { + const char c = *bp; + if (c == ']') { + break; + } + if (c <= '9' && c >= '0') { + num *= 10; + num += (c - '0'); + } else { + printf("array2 parse error.\n"); + return 1; + } + } + if (dim1) { + if (dim2) { + *dim1 = *dim2; + *dim2 = num; + } else { + *dim1 = num; + } + } + + return 1; +} + + +// ----------------------------------------------------- // +void bDNA::init(char *data, int len, bool swap) +{ + int *intPtr=0;short *shtPtr=0; + char *cp = 0;int dataLen =0; + //long nr=0; + intPtr = (int*)data; + + /* + SDNA (4 bytes) (magic number) + NAME (4 bytes) + (4 bytes) amount of names (int) + + + */ + + if (strncmp(data, "SDNA", 4)==0) + { + // skip ++ NAME + intPtr++; intPtr++; + } + + + + // Parse names + if (swap) + { + *intPtr = ChunkUtils::swapInt(*intPtr); + } + dataLen = *intPtr; + intPtr++; + + cp = (char*)intPtr; + int i; + for ( i=0; i amount of types (int) + + + */ + + intPtr = (int*)cp; + assert(strncmp(cp, "TYPE", 4)==0); intPtr++; + + if (swap) + { + *intPtr = ChunkUtils::swapInt(*intPtr); + } + dataLen = *intPtr; + intPtr++; + + cp = (char*)intPtr; + for ( i=0; i (short) the lengths of types + + */ + + // Parse type lens + intPtr = (int*)cp; + assert(strncmp(cp, "TLEN", 4)==0); intPtr++; + + dataLen = (int)mTypes.size(); + + shtPtr = (short*)intPtr; + for ( i=0; i amount of structs (int) + + + + + + + */ + + intPtr = (int*)shtPtr; + cp = (char*)intPtr; + assert(strncmp(cp, "STRC", 4)==0); intPtr++; + + if (swap) + { + *intPtr = ChunkUtils::swapInt(*intPtr); + } + dataLen = *intPtr; + intPtr++; + + + shtPtr = (short*)intPtr; + for ( i=0; itypes_count; ++i) { + /* if (!bf->types[i].is_struct)*/ + { + printf("%3d: sizeof(%s%s)=%d", + i, + bf->types[i].is_struct ? "struct " : "atomic ", + bf->types[i].name, bf->types[i].size); + if (bf->types[i].is_struct) { + int j; + printf(", %d fields: { ", bf->types[i].fieldtypes_count); + for (j=0; jtypes[i].fieldtypes_count; ++j) { + printf("%s %s", + bf->types[bf->types[i].fieldtypes[j]].name, + bf->names[bf->types[i].fieldnames[j]]); + if (j == bf->types[i].fieldtypes_count-1) { + printf(";}"); + } else { + printf("; "); + } + } + } + printf("\n\n"); + + } + } +#endif + +} + + + + +//eof + + diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.h b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.h new file mode 100644 index 000000000000..6e60087cce3d --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3DNA.h @@ -0,0 +1,110 @@ +/* +bParse +Copyright (c) 2006-2009 Charlie C & Erwin Coumans http://gamekit.googlecode.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef __BDNA_H__ +#define __BDNA_H__ + + +#include "b3Common.h" + +namespace bParse { + + struct bNameInfo + { + char* m_name; + bool m_isPointer; + int m_dim0; + int m_dim1; + }; + + class bDNA + { + public: + bDNA(); + ~bDNA(); + + void init(char *data, int len, bool swap=false); + + int getArraySize(char* str); + int getArraySizeNew(short name) + { + const bNameInfo& nameInfo = m_Names[name]; + return nameInfo.m_dim0*nameInfo.m_dim1; + } + int getElementSize(short type, short name) + { + const bNameInfo& nameInfo = m_Names[name]; + int size = nameInfo.m_isPointer ? mPtrLen*nameInfo.m_dim0*nameInfo.m_dim1 : mTlens[type]*nameInfo.m_dim0*nameInfo.m_dim1; + return size; + } + + int getNumNames() const + { + return m_Names.size(); + } + + char *getName(int ind); + char *getType(int ind); + short *getStruct(int ind); + short getLength(int ind); + int getReverseType(short type); + int getReverseType(const char *type); + + + int getNumStructs(); + + // + bool lessThan(bDNA* other); + + void initCmpFlags(bDNA *memDNA); + bool flagNotEqual(int dna_nr); + bool flagEqual(int dna_nr); + bool flagNone(int dna_nr); + + + int getPointerSize(); + + void dumpTypeDefinitions(); + + + private: + enum FileDNAFlags + { + FDF_NONE=0, + FDF_STRUCT_NEQU, + FDF_STRUCT_EQU + }; + + void initRecurseCmpFlags(int i); + + b3AlignedObjectArray mCMPFlags; + + b3AlignedObjectArray m_Names; + b3AlignedObjectArray mTypes; + b3AlignedObjectArray mStructs; + b3AlignedObjectArray mTlens; + b3HashMap mStructReverse; + b3HashMap mTypeLookup; + + int mPtrLen; + + + + + }; +} + + +#endif//__BDNA_H__ diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Defines.h b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Defines.h new file mode 100644 index 000000000000..8f28d3c44179 --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Defines.h @@ -0,0 +1,136 @@ +/* Copyright (C) 2006-2009 Charlie C & Erwin Coumans http://gamekit.googlecode.com +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef __B_DEFINES_H__ +#define __B_DEFINES_H__ + + +// MISC defines, see BKE_global.h, BKE_utildefines.h +#define B3_SIZEOFBLENDERHEADER 12 + + +// ------------------------------------------------------------ +#if defined(__sgi) || defined (__sparc) || defined (__sparc__) || defined (__PPC__) || defined (__ppc__) || defined (__BIG_ENDIAN__) +# define B3_MAKE_ID(a,b,c,d) ( (int)(a)<<24 | (int)(b)<<16 | (c)<<8 | (d) ) +#else +# define B3_MAKE_ID(a,b,c,d) ( (int)(d)<<24 | (int)(c)<<16 | (b)<<8 | (a) ) +#endif + + +// ------------------------------------------------------------ +#if defined(__sgi) || defined(__sparc) || defined(__sparc__) || defined (__PPC__) || defined (__ppc__) || defined (__BIG_ENDIAN__) +# define B3_MAKE_ID2(c, d) ( (c)<<8 | (d) ) +#else +# define B3_MAKE_ID2(c, d) ( (d)<<8 | (c) ) +#endif + +// ------------------------------------------------------------ +#define B3_ID_SCE B3_MAKE_ID2('S', 'C') +#define B3_ID_LI B3_MAKE_ID2('L', 'I') +#define B3_ID_OB B3_MAKE_ID2('O', 'B') +#define B3_ID_ME B3_MAKE_ID2('M', 'E') +#define B3_ID_CU B3_MAKE_ID2('C', 'U') +#define B3_ID_MB B3_MAKE_ID2('M', 'B') +#define B3_ID_MA B3_MAKE_ID2('M', 'A') +#define B3_ID_TE B3_MAKE_ID2('T', 'E') +#define B3_ID_IM B3_MAKE_ID2('I', 'M') +#define B3_ID_IK B3_MAKE_ID2('I', 'K') +#define B3_ID_WV B3_MAKE_ID2('W', 'V') +#define B3_ID_LT B3_MAKE_ID2('L', 'T') +#define B3_ID_SE B3_MAKE_ID2('S', 'E') +#define B3_ID_LF B3_MAKE_ID2('L', 'F') +#define B3_ID_LA B3_MAKE_ID2('L', 'A') +#define B3_ID_CA B3_MAKE_ID2('C', 'A') +#define B3_ID_IP B3_MAKE_ID2('I', 'P') +#define B3_ID_KE B3_MAKE_ID2('K', 'E') +#define B3_ID_WO B3_MAKE_ID2('W', 'O') +#define B3_ID_SCR B3_MAKE_ID2('S', 'R') +#define B3_ID_VF B3_MAKE_ID2('V', 'F') +#define B3_ID_TXT B3_MAKE_ID2('T', 'X') +#define B3_ID_SO B3_MAKE_ID2('S', 'O') +#define B3_ID_SAMPLE B3_MAKE_ID2('S', 'A') +#define B3_ID_GR B3_MAKE_ID2('G', 'R') +#define B3_ID_ID B3_MAKE_ID2('I', 'D') +#define B3_ID_AR B3_MAKE_ID2('A', 'R') +#define B3_ID_AC B3_MAKE_ID2('A', 'C') +#define B3_ID_SCRIPT B3_MAKE_ID2('P', 'Y') +#define B3_ID_FLUIDSIM B3_MAKE_ID2('F', 'S') +#define B3_ID_NT B3_MAKE_ID2('N', 'T') +#define B3_ID_BR B3_MAKE_ID2('B', 'R') + + +#define B3_ID_SEQ B3_MAKE_ID2('S', 'Q') +#define B3_ID_CO B3_MAKE_ID2('C', 'O') +#define B3_ID_PO B3_MAKE_ID2('A', 'C') +#define B3_ID_NLA B3_MAKE_ID2('N', 'L') + +#define B3_ID_VS B3_MAKE_ID2('V', 'S') +#define B3_ID_VN B3_MAKE_ID2('V', 'N') + + +// ------------------------------------------------------------ +#define B3_FORM B3_MAKE_ID('F','O','R','M') +#define B3_DDG1 B3_MAKE_ID('3','D','G','1') +#define B3_DDG2 B3_MAKE_ID('3','D','G','2') +#define B3_DDG3 B3_MAKE_ID('3','D','G','3') +#define B3_DDG4 B3_MAKE_ID('3','D','G','4') +#define B3_GOUR B3_MAKE_ID('G','O','U','R') +#define B3_BLEN B3_MAKE_ID('B','L','E','N') +#define B3_DER_ B3_MAKE_ID('D','E','R','_') +#define B3_V100 B3_MAKE_ID('V','1','0','0') +#define B3_DATA B3_MAKE_ID('D','A','T','A') +#define B3_GLOB B3_MAKE_ID('G','L','O','B') +#define B3_IMAG B3_MAKE_ID('I','M','A','G') +#define B3_TEST B3_MAKE_ID('T','E','S','T') +#define B3_USER B3_MAKE_ID('U','S','E','R') + + +// ------------------------------------------------------------ +#define B3_DNA1 B3_MAKE_ID('D','N','A','1') +#define B3_REND B3_MAKE_ID('R','E','N','D') +#define B3_ENDB B3_MAKE_ID('E','N','D','B') +#define B3_NAME B3_MAKE_ID('N','A','M','E') +#define B3_SDNA B3_MAKE_ID('S','D','N','A') +#define B3_TYPE B3_MAKE_ID('T','Y','P','E') +#define B3_TLEN B3_MAKE_ID('T','L','E','N') +#define B3_STRC B3_MAKE_ID('S','T','R','C') + + +// ------------------------------------------------------------ +#define B3_SWITCH_INT(a) { \ + char s_i, *p_i; \ + p_i= (char *)&(a); \ + s_i=p_i[0]; p_i[0]=p_i[3]; p_i[3]=s_i; \ + s_i=p_i[1]; p_i[1]=p_i[2]; p_i[2]=s_i; } + +// ------------------------------------------------------------ +#define B3_SWITCH_SHORT(a) { \ + char s_i, *p_i; \ + p_i= (char *)&(a); \ + s_i=p_i[0]; p_i[0]=p_i[1]; p_i[1]=s_i; } + +// ------------------------------------------------------------ +#define B3_SWITCH_LONGINT(a) { \ + char s_i, *p_i; \ + p_i= (char *)&(a); \ + s_i=p_i[0]; p_i[0]=p_i[7]; p_i[7]=s_i; \ + s_i=p_i[1]; p_i[1]=p_i[6]; p_i[6]=s_i; \ + s_i=p_i[2]; p_i[2]=p_i[5]; p_i[5]=s_i; \ + s_i=p_i[3]; p_i[3]=p_i[4]; p_i[4]=s_i; } + +#endif//__B_DEFINES_H__ diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp new file mode 100644 index 000000000000..432f7fc2b4bc --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp @@ -0,0 +1,1739 @@ +/* +bParse +Copyright (c) 2006-2009 Charlie C & Erwin Coumans http://gamekit.googlecode.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 "b3File.h" +#include "b3Common.h" +#include "b3Chunk.h" +#include "b3DNA.h" +#include +#include +#include +#include "b3Defines.h" +#include "Bullet3Serialize/Bullet2FileLoader/b3Serializer.h" +#include "Bullet3Common/b3AlignedAllocator.h" +#include "Bullet3Common/b3MinMax.h" + +#define B3_SIZEOFBLENDERHEADER 12 +#define MAX_ARRAY_LENGTH 512 +using namespace bParse; +#define MAX_STRLEN 1024 + +const char* getCleanName(const char* memName, char* buffer) +{ + int slen = strlen(memName); + assert(slen 0) + { + if (strncmp((tempBuffer + ChunkUtils::getOffset(mFlags)), "SDNANAME", 8) ==0) + dna.oldPtr = (tempBuffer + ChunkUtils::getOffset(mFlags)); + else dna.oldPtr = 0; + } + else dna.oldPtr = 0; + } + // Some Bullet files are missing the DNA1 block + // In Blender it's DNA1 + ChunkUtils::getOffset() + SDNA + NAME + // In Bullet tests its SDNA + NAME + else if (strncmp(tempBuffer, "SDNANAME", 8) ==0) + { + dna.oldPtr = blenderData + i; + dna.len = mFileLen-i; + + // Also no REND block, so exit now. + if (mVersion==276) break; + } + + if (mDataStart && dna.oldPtr) break; + tempBuffer++; + } + if (!dna.oldPtr || !dna.len) + { + //printf("Failed to find DNA1+SDNA pair\n"); + mFlags &= ~FD_OK; + return; + } + + + mFileDNA = new bDNA(); + + + ///mFileDNA->init will convert part of DNA file endianness to current CPU endianness if necessary + mFileDNA->init((char*)dna.oldPtr, dna.len, (mFlags & FD_ENDIAN_SWAP)!=0); + + + if (mVersion==276) + { + int i; + for (i=0;igetNumNames();i++) + { + if (strcmp(mFileDNA->getName(i),"int")==0) + { + mFlags |= FD_BROKEN_DNA; + } + } + if ((mFlags&FD_BROKEN_DNA)!=0) + { + //printf("warning: fixing some broken DNA version\n"); + } + } + + + + if (verboseMode & FD_VERBOSE_DUMP_DNA_TYPE_DEFINITIONS) + mFileDNA->dumpTypeDefinitions(); + + mMemoryDNA = new bDNA(); + int littleEndian= 1; + littleEndian= ((char*)&littleEndian)[0]; + + mMemoryDNA->init(memDna,memDnaLength,littleEndian==0); + + + + + ///@todo we need a better version check, add version/sub version info from FileGlobal into memory DNA/header files + if (mMemoryDNA->getNumNames() != mFileDNA->getNumNames()) + { + mFlags |= FD_VERSION_VARIES; + //printf ("Warning, file DNA is different than built in, performance is reduced. Best to re-export file with a matching version/platform"); + } + + // as long as it kept up to date it will be ok!! + if (mMemoryDNA->lessThan(mFileDNA)) + { + //printf ("Warning, file DNA is newer than built in."); + } + + + mFileDNA->initCmpFlags(mMemoryDNA); + + parseData(); + + resolvePointers(verboseMode); + + updateOldPointers(); + + +} + + + +// ----------------------------------------------------- // +void bFile::swap(char *head, bChunkInd& dataChunk, bool ignoreEndianFlag) +{ + char *data = head; + short *strc = mFileDNA->getStruct(dataChunk.dna_nr); + + + + const char s[] = "SoftBodyMaterialData"; + int szs = sizeof(s); + if (strncmp((char*)&dataChunk.code,"ARAY",4)==0) + { + short *oldStruct = mFileDNA->getStruct(dataChunk.dna_nr); + char *oldType = mFileDNA->getType(oldStruct[0]); + if (strncmp(oldType,s,szs)==0) + { + return; + } + } + + + int len = mFileDNA->getLength(strc[0]); + + for (int i=0; icode & 0xFFFF)==0) + c->code >>=16; + B3_SWITCH_INT(c->len); + B3_SWITCH_INT(c->dna_nr); + B3_SWITCH_INT(c->nr); + } else + { + bChunkPtr8* c = (bChunkPtr8*) dataPtr; + if ((c->code & 0xFFFF)==0) + c->code >>=16; + B3_SWITCH_INT(c->len); + B3_SWITCH_INT(c->dna_nr); + B3_SWITCH_INT(c->nr); + + } + } else + { + if (mFlags &FD_BITS_VARIES) + { + bChunkPtr8*c = (bChunkPtr8*) dataPtr; + if ((c->code & 0xFFFF)==0) + c->code >>=16; + B3_SWITCH_INT(c->len); + B3_SWITCH_INT(c->dna_nr); + B3_SWITCH_INT(c->nr); + + } else + { + bChunkPtr4* c = (bChunkPtr4*) dataPtr; + if ((c->code & 0xFFFF)==0) + c->code >>=16; + B3_SWITCH_INT(c->len); + + B3_SWITCH_INT(c->dna_nr); + B3_SWITCH_INT(c->nr); + + } + } + +} + + +void bFile::swapDNA(char* ptr) +{ + bool swap = ((mFlags & FD_ENDIAN_SWAP)!=0); + + char* data = &ptr[20]; +// void bDNA::init(char *data, int len, bool swap) + int *intPtr=0;short *shtPtr=0; + char *cp = 0;int dataLen =0; + //long nr=0; + intPtr = (int*)data; + + /* + SDNA (4 bytes) (magic number) + NAME (4 bytes) + (4 bytes) amount of names (int) + + + */ + + if (strncmp(data, "SDNA", 4)==0) + { + // skip ++ NAME + intPtr++; intPtr++; + } + + + + // Parse names + if (swap) + dataLen = ChunkUtils::swapInt(*intPtr); + else + dataLen = *intPtr; + + *intPtr = ChunkUtils::swapInt(*intPtr); + intPtr++; + + cp = (char*)intPtr; + int i; + for ( i=0; i amount of types (int) + + + */ + + intPtr = (int*)cp; + assert(strncmp(cp, "TYPE", 4)==0); intPtr++; + + if (swap) + dataLen = ChunkUtils::swapInt(*intPtr); + else + dataLen = *intPtr; + + *intPtr = ChunkUtils::swapInt(*intPtr); + + intPtr++; + + cp = (char*)intPtr; + for ( i=0; i (short) the lengths of types + + */ + + // Parse type lens + intPtr = (int*)cp; + assert(strncmp(cp, "TLEN", 4)==0); intPtr++; + + + shtPtr = (short*)intPtr; + for ( i=0; i amount of structs (int) + + + + + + + */ + + intPtr = (int*)shtPtr; + cp = (char*)intPtr; + assert(strncmp(cp, "STRC", 4)==0); + intPtr++; + + if (swap) + dataLen = ChunkUtils::swapInt(*intPtr); + else + dataLen = *intPtr; + + *intPtr = ChunkUtils::swapInt(*intPtr); + + intPtr++; + + + shtPtr = (short*)intPtr; + for ( i=0; i=0) + { + swap(dataPtrHead, dataChunk,ignoreEndianFlag); + } else + { + //printf("unknown chunk\n"); + } + } + + // next please! + dataPtr += seek; + + seek = getNextBlock(&dataChunk, dataPtr, mFlags); + if (seek < 0) + break; + } + + if (mFlags & FD_ENDIAN_SWAP) + { + mFlags &= ~FD_ENDIAN_SWAP; + } else + { + mFlags |= FD_ENDIAN_SWAP; + } + + + +} + + +// ----------------------------------------------------- // +char* bFile::readStruct(char *head, bChunkInd& dataChunk) +{ + bool ignoreEndianFlag = false; + + if (mFlags & FD_ENDIAN_SWAP) + swap(head, dataChunk, ignoreEndianFlag); + + + + if (!mFileDNA->flagEqual(dataChunk.dna_nr)) + { + // Ouch! need to rebuild the struct + short *oldStruct,*curStruct; + char *oldType, *newType; + int oldLen, curLen, reverseOld; + + + oldStruct = mFileDNA->getStruct(dataChunk.dna_nr); + oldType = mFileDNA->getType(oldStruct[0]); + + oldLen = mFileDNA->getLength(oldStruct[0]); + + if ((mFlags&FD_BROKEN_DNA)!=0) + { + if ((strcmp(oldType,"b3QuantizedBvhNodeData")==0)&&oldLen==20) + { + return 0; + } + if ((strcmp(oldType,"b3ShortIntIndexData")==0)) + { + int allocLen = 2; + char *dataAlloc = new char[(dataChunk.nr*allocLen)+1]; + memset(dataAlloc, 0, (dataChunk.nr*allocLen)+1); + short* dest = (short*) dataAlloc; + const short* src = (short*) head; + for (int i=0;igetReverseType(oldType); + + if ((reverseOld!=-1)) + { + // make sure it's here + //assert(reverseOld!= -1 && "getReverseType() returned -1, struct required!"); + + // + curStruct = mMemoryDNA->getStruct(reverseOld); + newType = mMemoryDNA->getType(curStruct[0]); + curLen = mMemoryDNA->getLength(curStruct[0]); + + + + // make sure it's the same + assert((strcmp(oldType, newType)==0) && "internal error, struct mismatch!"); + + + // numBlocks * length + + int allocLen = (curLen); + char *dataAlloc = new char[(dataChunk.nr*allocLen)+1]; + memset(dataAlloc, 0, (dataChunk.nr*allocLen)); + + // track allocated + addDataBlock(dataAlloc); + + char *cur = dataAlloc; + char *old = head; + for (int block=0; blockgetStruct(dataChunk.dna_nr); + oldType = mFileDNA->getType(oldStruct[0]); + printf("%s equal structure, just memcpy\n",oldType); +#endif // + } + + + char *dataAlloc = new char[(dataChunk.len)+1]; + memset(dataAlloc, 0, dataChunk.len+1); + + + // track allocated + addDataBlock(dataAlloc); + + memcpy(dataAlloc, head, dataChunk.len); + return dataAlloc; + +} + + +// ----------------------------------------------------- // +void bFile::parseStruct(char *strcPtr, char *dtPtr, int old_dna, int new_dna, bool fixupPointers) +{ + if (old_dna == -1) return; + if (new_dna == -1) return; + + //disable this, because we need to fixup pointers/ListBase + if (0)//mFileDNA->flagEqual(old_dna)) + { + short *strc = mFileDNA->getStruct(old_dna); + int len = mFileDNA->getLength(strc[0]); + + memcpy(strcPtr, dtPtr, len); + return; + } + + // Ok, now build the struct + char *memType, *memName, *cpc, *cpo; + short *fileStruct, *filePtrOld, *memoryStruct, *firstStruct; + int elementLength, size, revType, old_nr, new_nr, fpLen; + short firstStructType; + + + // File to memory lookup + memoryStruct = mMemoryDNA->getStruct(new_dna); + fileStruct = mFileDNA->getStruct(old_dna); + firstStruct = fileStruct; + + + filePtrOld = fileStruct; + firstStructType = mMemoryDNA->getStruct(0)[0]; + + // Get number of elements + elementLength = memoryStruct[1]; + memoryStruct+=2; + + cpc = strcPtr; cpo = 0; + for (int ele=0; elegetType(memoryStruct[0]); + memName = mMemoryDNA->getName(memoryStruct[1]); + + + size = mMemoryDNA->getElementSize(memoryStruct[0], memoryStruct[1]); + revType = mMemoryDNA->getReverseType(memoryStruct[0]); + + if (revType != -1 && memoryStruct[0]>=firstStructType && memName[0] != '*') + { + cpo = getFileElement(firstStruct, memName, memType, dtPtr, &filePtrOld); + if (cpo) + { + int arrayLen = mFileDNA->getArraySizeNew(filePtrOld[1]); + old_nr = mFileDNA->getReverseType(memType); + new_nr = revType; + fpLen = mFileDNA->getElementSize(filePtrOld[0], filePtrOld[1]); + if (arrayLen==1) + { + parseStruct(cpc, cpo, old_nr, new_nr,fixupPointers); + } else + { + char* tmpCpc = cpc; + char* tmpCpo = cpo; + + for (int i=0;i3 && type <8) + { + char c; + char *cp = data; + for (int i=0; igetPointerSize(); + int ptrMem = mMemoryDNA->getPointerSize(); + + if (!src && !dst) + return; + + + if (ptrFile == ptrMem) + { + memcpy(dst, src, ptrMem); + } + else if (ptrMem==4 && ptrFile==8) + { + b3PointerUid* oldPtr = (b3PointerUid*)src; + b3PointerUid* newPtr = (b3PointerUid*)dst; + + if (oldPtr->m_uniqueIds[0] == oldPtr->m_uniqueIds[1]) + { + //Bullet stores the 32bit unique ID in both upper and lower part of 64bit pointers + //so it can be used to distinguish between .blend and .bullet + newPtr->m_uniqueIds[0] = oldPtr->m_uniqueIds[0]; + } else + { + //deal with pointers the Blender .blend style way, see + //readfile.c in the Blender source tree + b3Long64 longValue = *((b3Long64*)src); + //endian swap for 64bit pointer otherwise truncation will fail due to trailing zeros + if (mFlags & FD_ENDIAN_SWAP) + B3_SWITCH_LONGINT(longValue); + *((int*)dst) = (int)(longValue>>3); + } + + } + else if (ptrMem==8 && ptrFile==4) + { + b3PointerUid* oldPtr = (b3PointerUid*)src; + b3PointerUid* newPtr = (b3PointerUid*)dst; + if (oldPtr->m_uniqueIds[0] == oldPtr->m_uniqueIds[1]) + { + newPtr->m_uniqueIds[0] = oldPtr->m_uniqueIds[0]; + newPtr->m_uniqueIds[1] = 0; + } else + { + *((b3Long64*)dst)= *((int*)src); + } + } + else + { + printf ("%d %d\n", ptrFile,ptrMem); + assert(0 && "Invalid pointer len"); + } + + +} + + +// ----------------------------------------------------- // +void bFile::getMatchingFileDNA(short* dna_addr, const char* lookupName, const char* lookupType, char *strcData, char *data, bool fixupPointers) +{ + // find the matching memory dna data + // to the file being loaded. Fill the + // memory with the file data... + + int len = dna_addr[1]; + dna_addr+=2; + + for (int i=0; igetType(dna_addr[0]); + const char* name = mFileDNA->getName(dna_addr[1]); + + + + int eleLen = mFileDNA->getElementSize(dna_addr[0], dna_addr[1]); + + if ((mFlags&FD_BROKEN_DNA)!=0) + { + if ((strcmp(type,"short")==0)&&(strcmp(name,"int")==0)) + { + eleLen = 0; + } + } + + if (strcmp(lookupName, name)==0) + { + //int arrayLenold = mFileDNA->getArraySize((char*)name.c_str()); + int arrayLen = mFileDNA->getArraySizeNew(dna_addr[1]); + //assert(arrayLenold == arrayLen); + + if (name[0] == '*') + { + // cast pointers + int ptrFile = mFileDNA->getPointerSize(); + int ptrMem = mMemoryDNA->getPointerSize(); + safeSwapPtr(strcData,data); + + if (fixupPointers) + { + if (arrayLen > 1) + { + //void **sarray = (void**)strcData; + //void **darray = (void**)data; + + char *cpc, *cpo; + cpc = (char*)strcData; + cpo = (char*)data; + + for (int a=0; agetStruct(old_nr); + int elementLength = old[1]; + old+=2; + + for (int i=0; igetType(old[0]); + char* name = mFileDNA->getName(old[1]); + int len = mFileDNA->getElementSize(old[0], old[1]); + + if (strcmp(lookupName, name)==0) + { + if (strcmp(type, lookupType)==0) + { + if (foundPos) + *foundPos = old; + return data; + } + return 0; + } + data+=len; + } + return 0; +} + + +// ----------------------------------------------------- // +void bFile::swapStruct(int dna_nr, char *data,bool ignoreEndianFlag) +{ + if (dna_nr == -1) return; + + short *strc = mFileDNA->getStruct(dna_nr); + //short *firstStrc = strc; + + int elementLen= strc[1]; + strc+=2; + + short first = mFileDNA->getStruct(0)[0]; + + char *buf = data; + for (int i=0; igetType(strc[0]); + char *name = mFileDNA->getName(strc[1]); + + int size = mFileDNA->getElementSize(strc[0], strc[1]); + if (strc[0] >= first && name[0]!='*') + { + int old_nr = mFileDNA->getReverseType(type); + int arrayLen = mFileDNA->getArraySizeNew(strc[1]); + if (arrayLen==1) + { + swapStruct(old_nr,buf,ignoreEndianFlag); + } else + { + char* tmpBuf = buf; + for (int i=0;igetArraySize(name); + int arrayLen = mFileDNA->getArraySizeNew(strc[1]); + //assert(arrayLenOld == arrayLen); + swapData(buf, strc[0], arrayLen,ignoreEndianFlag); + } + buf+=size; + } +} + +void bFile::resolvePointersMismatch() +{ +// printf("resolvePointersStructMismatch\n"); + + int i; + + for (i=0;i< m_pointerFixupArray.size();i++) + { + char* cur = m_pointerFixupArray.at(i); + void** ptrptr = (void**) cur; + void* ptr = *ptrptr; + ptr = findLibPointer(ptr); + if (ptr) + { + //printf("Fixup pointer!\n"); + *(ptrptr) = ptr; + } else + { +// printf("pointer not found: %x\n",cur); + } + } + + + for (i=0; igetPointerSize(); + int ptrFile = mFileDNA->getPointerSize(); + + + int blockLen = block->len / ptrFile; + + void *onptr = findLibPointer(*ptrptr); + if (onptr) + { + char *newPtr = new char[blockLen * ptrMem]; + addDataBlock(newPtr); + memset(newPtr, 0, blockLen * ptrMem); + + void **onarray = (void**)onptr; + char *oldPtr = (char*)onarray; + + int p = 0; + while (blockLen-- > 0) + { + b3PointerUid dp = {{0}}; + safeSwapPtr((char*)dp.m_uniqueIds, oldPtr); + + void **tptr = (void**)(newPtr + p * ptrMem); + *tptr = findLibPointer(dp.m_ptr); + + oldPtr += ptrFile; + ++p; + } + + *ptrptr = newPtr; + } + } + } +} + + +///this loop only works fine if the Blender DNA structure of the file matches the headerfiles +void bFile::resolvePointersChunk(const bChunkInd& dataChunk, int verboseMode) +{ + bParse::bDNA* fileDna = mFileDNA ? mFileDNA : mMemoryDNA; + + short int* oldStruct = fileDna->getStruct(dataChunk.dna_nr); + short oldLen = fileDna->getLength(oldStruct[0]); + //char* structType = fileDna->getType(oldStruct[0]); + + char* cur = (char*)findLibPointer(dataChunk.oldPtr); + for (int block=0; blockgetStruct(0)[0]; + + + char* elemPtr= strcPtr; + + short int* oldStruct = fileDna->getStruct(dna_nr); + + int elementLength = oldStruct[1]; + oldStruct+=2; + + int totalSize = 0; + + for (int ele=0; elegetType(oldStruct[0]); + memName = fileDna->getName(oldStruct[1]); + + + + int arrayLen = fileDna->getArraySizeNew(oldStruct[1]); + if (memName[0] == '*') + { + if (arrayLen > 1) + { + void **array= (void**)elemPtr; + for (int a=0; a ",&memName[1]); + printf("%p ", array[a]); + printf("\n",&memName[1]); + } + + array[a] = findLibPointer(array[a]); + } + } + else + { + void** ptrptr = (void**) elemPtr; + void* ptr = *ptrptr; + if (verboseMode & FD_VERBOSE_EXPORT_XML) + { + for (int i=0;i ",&memName[1]); + printf("%p ", ptr); + printf("\n",&memName[1]); + } + ptr = findLibPointer(ptr); + + if (ptr) + { + // printf("Fixup pointer at 0x%x from 0x%x to 0x%x!\n",ptrptr,*ptrptr,ptr); + *(ptrptr) = ptr; + if (memName[1] == '*' && ptrptr && *ptrptr) + { + // This will only work if the given **array is continuous + void **array= (void**)*(ptrptr); + void *np= array[0]; + int n=0; + while (np) + { + np= findLibPointer(array[n]); + if (np) array[n]= np; + n++; + } + } + } else + { + // printf("Cannot fixup pointer at 0x%x from 0x%x to 0x%x!\n",ptrptr,*ptrptr,ptr); + } + } + } else + { + int revType = fileDna->getReverseType(oldStruct[0]); + if (oldStruct[0]>=firstStructType) //revType != -1 && + { + char cleanName[MAX_STRLEN]; + getCleanName(memName,cleanName); + + int arrayLen = fileDna->getArraySizeNew(oldStruct[1]); + int byteOffset = 0; + + if (verboseMode & FD_VERBOSE_EXPORT_XML) + { + for (int i=0;i1) + { + printf("<%s type=\"%s\" count=%d>\n",cleanName,memType, arrayLen); + } else + { + printf("<%s type=\"%s\">\n",cleanName,memType); + } + } + + for (int i=0;i\n",cleanName); + } + } else + { + //export a simple type + if (verboseMode & FD_VERBOSE_EXPORT_XML) + { + + if (arrayLen>MAX_ARRAY_LENGTH) + { + printf("too long\n"); + } else + { + //printf("%s %s\n",memType,memName); + + bool isIntegerType = (strcmp(memType,"char")==0) || (strcmp(memType,"int")==0) || (strcmp(memType,"short")==0); + + if (isIntegerType) + { + const char* newtype="int"; + int dbarray[MAX_ARRAY_LENGTH]; + int* dbPtr = 0; + char* tmp = elemPtr; + dbPtr = &dbarray[0]; + if (dbPtr) + { + char cleanName[MAX_STRLEN]; + getCleanName(memName,cleanName); + + int i; + getElement(arrayLen, newtype,memType, tmp, (char*)dbPtr); + for (i=0;i",cleanName,memType); + else + printf("<%s type=\"%s\" count=%d>",cleanName,memType,arrayLen); + for (i=0;i\n",cleanName); + } + } else + { + const char* newtype="double"; + double dbarray[MAX_ARRAY_LENGTH]; + double* dbPtr = 0; + char* tmp = elemPtr; + dbPtr = &dbarray[0]; + if (dbPtr) + { + int i; + getElement(arrayLen, newtype,memType, tmp, (char*)dbPtr); + for (i=0;i",memName,memType); + } + else + { + printf("<%s type=\"%s\" count=%d>",cleanName,memType,arrayLen); + } + for (i=0;i\n",cleanName); + } + } + } + + } + } + } + + int size = fileDna->getElementSize(oldStruct[0], oldStruct[1]); + totalSize += size; + elemPtr+=size; + + } + + return totalSize; +} + + +///Resolve pointers replaces the original pointers in structures, and linked lists by the new in-memory structures +void bFile::resolvePointers(int verboseMode) +{ + bParse::bDNA* fileDna = mFileDNA ? mFileDNA : mMemoryDNA; + + //char *dataPtr = mFileBuffer+mDataStart; + + if (1) //mFlags & (FD_BITS_VARIES | FD_VERSION_VARIES)) + { + resolvePointersMismatch(); + } + + { + + if (verboseMode & FD_VERBOSE_EXPORT_XML) + { + printf("\n"); + int numitems = m_chunks.size(); + printf("\n", b3GetVersion(), numitems); + } + for (int i=0;iflagEqual(dataChunk.dna_nr)) + { + //dataChunk.len + short int* oldStruct = fileDna->getStruct(dataChunk.dna_nr); + char* oldType = fileDna->getType(oldStruct[0]); + + if (verboseMode & FD_VERBOSE_EXPORT_XML) + printf(" <%s pointer=%p>\n",oldType,dataChunk.oldPtr); + + resolvePointersChunk(dataChunk, verboseMode); + + if (verboseMode & FD_VERBOSE_EXPORT_XML) + printf(" \n",oldType); + } else + { + //printf("skipping mStruct\n"); + } + } + if (verboseMode & FD_VERBOSE_EXPORT_XML) + { + printf("\n"); + } + } + + +} + + +// ----------------------------------------------------- // +void* bFile::findLibPointer(void *ptr) +{ + + bStructHandle** ptrptr = getLibPointers().find(ptr); + if (ptrptr) + return *ptrptr; + return 0; +} + + +void bFile::updateOldPointers() +{ + int i; + + for (i=0;igetStruct(dataChunk.dna_nr); + char* typeName = dna->getType(newStruct[0]); + printf("%3d: %s ",i,typeName); + + printf("code=%s ",codestr); + + printf("ptr=%p ",dataChunk.oldPtr); + printf("len=%d ",dataChunk.len); + printf("nr=%d ",dataChunk.nr); + if (dataChunk.nr!=1) + { + printf("not 1\n"); + } + printf("\n"); + + + + + } + +#if 0 + IDFinderData ifd; + ifd.success = 0; + ifd.IDname = NULL; + ifd.just_print_it = 1; + for (i=0; im_blocks.size(); ++i) + { + BlendBlock* bb = bf->m_blocks[i]; + printf("tag='%s'\tptr=%p\ttype=%s\t[%4d]", bb->tag, bb,bf->types[bb->type_index].name,bb->m_array_entries_.size()); + block_ID_finder(bb, bf, &ifd); + printf("\n"); + } +#endif + +} + + +void bFile::writeChunks(FILE* fp, bool fixupPointers) +{ + bParse::bDNA* fileDna = mFileDNA ? mFileDNA : mMemoryDNA; + + for (int i=0;igetStruct(dataChunk.dna_nr); + oldType = fileDna->getType(oldStruct[0]); + oldLen = fileDna->getLength(oldStruct[0]); + ///don't try to convert Link block data, just memcpy it. Other data can be converted. + reverseOld = mMemoryDNA->getReverseType(oldType); + + + if ((reverseOld!=-1)) + { + // make sure it's here + //assert(reverseOld!= -1 && "getReverseType() returned -1, struct required!"); + // + curStruct = mMemoryDNA->getStruct(reverseOld); + newType = mMemoryDNA->getType(curStruct[0]); + // make sure it's the same + assert((strcmp(oldType, newType)==0) && "internal error, struct mismatch!"); + + + curLen = mMemoryDNA->getLength(curStruct[0]); + dataChunk.dna_nr = reverseOld; + if (strcmp("Link",oldType)!=0) + { + dataChunk.len = curLen * dataChunk.nr; + } else + { +// printf("keep length of link = %d\n",dataChunk.len); + } + + //write the structure header + fwrite(&dataChunk,sizeof(bChunkInd),1,fp); + + + + short int* curStruct1; + curStruct1 = mMemoryDNA->getStruct(dataChunk.dna_nr); + assert(curStruct1 == curStruct); + + char* cur = fixupPointers ? (char*)findLibPointer(dataChunk.oldPtr) : (char*)dataChunk.oldPtr; + + //write the actual contents of the structure(s) + fwrite(cur,dataChunk.len,1,fp); + } else + { + printf("serious error, struct mismatch: don't write\n"); + } + } + +} + + +// ----------------------------------------------------- // +int bFile::getNextBlock(bChunkInd *dataChunk, const char *dataPtr, const int flags) +{ + bool swap = false; + bool varies = false; + + if (flags &FD_ENDIAN_SWAP) + swap = true; + if (flags &FD_BITS_VARIES) + varies = true; + + if (VOID_IS_8) + { + if (varies) + { + bChunkPtr4 head; + memcpy(&head, dataPtr, sizeof(bChunkPtr4)); + + + bChunkPtr8 chunk; + + chunk.code = head.code; + chunk.len = head.len; + chunk.m_uniqueInts[0] = head.m_uniqueInt; + chunk.m_uniqueInts[1] = 0; + chunk.dna_nr = head.dna_nr; + chunk.nr = head.nr; + + if (swap) + { + if ((chunk.code & 0xFFFF)==0) + chunk.code >>=16; + + B3_SWITCH_INT(chunk.len); + B3_SWITCH_INT(chunk.dna_nr); + B3_SWITCH_INT(chunk.nr); + } + + + memcpy(dataChunk, &chunk, sizeof(bChunkInd)); + } + else + { + bChunkPtr8 c; + memcpy(&c, dataPtr, sizeof(bChunkPtr8)); + + if (swap) + { + if ((c.code & 0xFFFF)==0) + c.code >>=16; + + B3_SWITCH_INT(c.len); + B3_SWITCH_INT(c.dna_nr); + B3_SWITCH_INT(c.nr); + } + + memcpy(dataChunk, &c, sizeof(bChunkInd)); + } + } + else + { + if (varies) + { + bChunkPtr8 head; + memcpy(&head, dataPtr, sizeof(bChunkPtr8)); + + + bChunkPtr4 chunk; + chunk.code = head.code; + chunk.len = head.len; + + if (head.m_uniqueInts[0]==head.m_uniqueInts[1]) + { + chunk.m_uniqueInt = head.m_uniqueInts[0]; + } else + { + b3Long64 oldPtr =0; + memcpy(&oldPtr, &head.m_uniqueInts[0], 8); + if (swap) + B3_SWITCH_LONGINT(oldPtr); + chunk.m_uniqueInt = (int)(oldPtr >> 3); + } + + + chunk.dna_nr = head.dna_nr; + chunk.nr = head.nr; + + if (swap) + { + if ((chunk.code & 0xFFFF)==0) + chunk.code >>=16; + + B3_SWITCH_INT(chunk.len); + B3_SWITCH_INT(chunk.dna_nr); + B3_SWITCH_INT(chunk.nr); + } + + memcpy(dataChunk, &chunk, sizeof(bChunkInd)); + } + else + { + bChunkPtr4 c; + memcpy(&c, dataPtr, sizeof(bChunkPtr4)); + + if (swap) + { + if ((c.code & 0xFFFF)==0) + c.code >>=16; + + B3_SWITCH_INT(c.len); + B3_SWITCH_INT(c.dna_nr); + B3_SWITCH_INT(c.nr); + } + memcpy(dataChunk, &c, sizeof(bChunkInd)); + } + } + + if (dataChunk->len < 0) + return -1; + +#if 0 + print ("----------"); + print (dataChunk->code); + print (dataChunk->len); + print (dataChunk->old); + print (dataChunk->dna_nr); + print (dataChunk->nr); +#endif + return (dataChunk->len+ChunkUtils::getOffset(flags)); +} + + + +//eof diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.h b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.h new file mode 100644 index 000000000000..861056806d78 --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3File.h @@ -0,0 +1,165 @@ +/* +bParse +Copyright (c) 2006-2009 Charlie C & Erwin Coumans http://gamekit.googlecode.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef __BFILE_H__ +#define __BFILE_H__ + +#include "b3Common.h" +#include "b3Chunk.h" +#include + +namespace bParse { + + // ----------------------------------------------------- // + enum bFileFlags + { + FD_INVALID =0, + FD_OK =1, + FD_VOID_IS_8 =2, + FD_ENDIAN_SWAP =4, + FD_FILE_64 =8, + FD_BITS_VARIES =16, + FD_VERSION_VARIES = 32, + FD_DOUBLE_PRECISION =64, + FD_BROKEN_DNA = 128 + }; + + enum bFileVerboseMode + { + FD_VERBOSE_EXPORT_XML = 1, + FD_VERBOSE_DUMP_DNA_TYPE_DEFINITIONS = 2, + FD_VERBOSE_DUMP_CHUNKS = 4, + FD_VERBOSE_DUMP_FILE_INFO=8, + }; + // ----------------------------------------------------- // + class bFile + { + protected: + + char m_headerString[7]; + + bool mOwnsBuffer; + char* mFileBuffer; + int mFileLen; + int mVersion; + + + bPtrMap mLibPointers; + + int mDataStart; + bDNA* mFileDNA; + bDNA* mMemoryDNA; + + b3AlignedObjectArray m_pointerFixupArray; + b3AlignedObjectArray m_pointerPtrFixupArray; + + b3AlignedObjectArray m_chunks; + b3HashMap m_chunkPtrPtrMap; + + // + + bPtrMap mDataPointers; + + + int mFlags; + + // //////////////////////////////////////////////////////////////////////////// + + // buffer offset util + int getNextBlock(bChunkInd *dataChunk, const char *dataPtr, const int flags); + void safeSwapPtr(char *dst, const char *src); + + virtual void parseHeader(); + + virtual void parseData() = 0; + + void resolvePointersMismatch(); + void resolvePointersChunk(const bChunkInd& dataChunk, int verboseMode); + + int resolvePointersStructRecursive(char *strcPtr, int old_dna, int verboseMode, int recursion); + //void swapPtr(char *dst, char *src); + + void parseStruct(char *strcPtr, char *dtPtr, int old_dna, int new_dna, bool fixupPointers); + void getMatchingFileDNA(short* old, const char* lookupName, const char* lookupType, char *strcData, char *data, bool fixupPointers); + char* getFileElement(short *firstStruct, char *lookupName, char *lookupType, char *data, short **foundPos); + + + void swap(char *head, class bChunkInd& ch, bool ignoreEndianFlag); + void swapData(char *data, short type, int arraySize, bool ignoreEndianFlag); + void swapStruct(int dna_nr, char *data, bool ignoreEndianFlag); + void swapLen(char *dataPtr); + void swapDNA(char* ptr); + + + char* readStruct(char *head, class bChunkInd& chunk); + char *getAsString(int code); + + void parseInternal(int verboseMode, char* memDna,int memDnaLength); + + public: + bFile(const char *filename, const char headerString[7]); + + //todo: make memoryBuffer const char + //bFile( const char *memoryBuffer, int len); + bFile( char *memoryBuffer, int len, const char headerString[7]); + virtual ~bFile(); + + bDNA* getFileDNA() + { + return mFileDNA; + } + + virtual void addDataBlock(char* dataBlock) = 0; + + int getFlags() const + { + return mFlags; + } + + bPtrMap& getLibPointers() + { + return mLibPointers; + } + + void* findLibPointer(void *ptr); + + bool ok(); + + virtual void parse(int verboseMode) = 0; + + virtual int write(const char* fileName, bool fixupPointers=false) = 0; + + virtual void writeChunks(FILE* fp, bool fixupPointers ); + + virtual void writeDNA(FILE* fp) = 0; + + void updateOldPointers(); + void resolvePointers(int verboseMode); + + void dumpChunks(bDNA* dna); + + int getVersion() const + { + return mVersion; + } + //pre-swap the endianness, so that data loaded on a target with different endianness doesn't need to be swapped + void preSwap(); + void writeFile(const char* fileName); + + }; +} + + +#endif//__BFILE_H__ diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.cpp b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.cpp new file mode 100644 index 000000000000..c6a2a832ad96 --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.cpp @@ -0,0 +1,908 @@ +char b3s_bulletDNAstr[]= { +char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(63),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), +char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95), +char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111), +char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(115),char(0),char(42),char(102),char(105),char(114),char(115),char(116),char(0),char(42),char(108),char(97),char(115), +char(116),char(0),char(109),char(95),char(102),char(108),char(111),char(97),char(116),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(108),char(91),char(51), +char(93),char(0),char(109),char(95),char(98),char(97),char(115),char(105),char(115),char(0),char(109),char(95),char(111),char(114),char(105),char(103),char(105),char(110),char(0),char(109), +char(95),char(114),char(111),char(111),char(116),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98), +char(116),char(114),char(101),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100), +char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(91),char(51),char(93),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122), +char(101),char(100),char(65),char(97),char(98),char(98),char(77),char(97),char(120),char(91),char(51),char(93),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77), +char(105),char(110),char(79),char(114),char(103),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77),char(97),char(120),char(79),char(114),char(103),char(0),char(109), +char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98),char(80),char(97), +char(114),char(116),char(0),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109), +char(95),char(112),char(97),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101), +char(120),char(79),char(114),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(98), +char(118),char(104),char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(0),char(109),char(95),char(98),char(118),char(104),char(65),char(97),char(98),char(98),char(77), +char(97),char(120),char(0),char(109),char(95),char(98),char(118),char(104),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110), +char(0),char(109),char(95),char(99),char(117),char(114),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(117),char(115), +char(101),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(110),char(117),char(109),char(67), +char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(76),char(101),char(97),char(102),char(78),char(111),char(100),char(101),char(115),char(0),char(109), +char(95),char(110),char(117),char(109),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117), +char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(99),char(111),char(110),char(116),char(105),char(103),char(117),char(111), +char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105), +char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116), +char(114),char(0),char(42),char(109),char(95),char(115),char(117),char(98),char(84),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(80),char(116),char(114),char(0), +char(109),char(95),char(116),char(114),char(97),char(118),char(101),char(114),char(115),char(97),char(108),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(110),char(117), +char(109),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(72),char(101),char(97),char(100),char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(110), +char(97),char(109),char(101),char(0),char(109),char(95),char(115),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(112),char(97), +char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110), +char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(83),char(99),char(97), +char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(101),char(78),char(111),char(114),char(109),char(97),char(108),char(0),char(109), +char(95),char(112),char(108),char(97),char(110),char(101),char(67),char(111),char(110),char(115),char(116),char(97),char(110),char(116),char(0),char(109),char(95),char(105),char(109),char(112), +char(108),char(105),char(99),char(105),char(116),char(83),char(104),char(97),char(112),char(101),char(68),char(105),char(109),char(101),char(110),char(115),char(105),char(111),char(110),char(115), +char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(109), +char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(111),char(115),char(0),char(109),char(95),char(114),char(97),char(100), +char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108), +char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(42),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(80),char(111), +char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(108),char(111),char(99), +char(97),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(83),char(105),char(122),char(101),char(0), +char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(0),char(109),char(95),char(112),char(97),char(100),char(91),char(50),char(93),char(0),char(109),char(95),char(118), +char(97),char(108),char(117),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(112),char(97),char(100),char(0),char(42),char(109),char(95),char(118),char(101), +char(114),char(116),char(105),char(99),char(101),char(115),char(51),char(102),char(0),char(42),char(109),char(95),char(118),char(101),char(114),char(116),char(105),char(99),char(101),char(115), +char(51),char(100),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(51),char(50),char(0),char(42),char(109),char(95),char(51), +char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(42),char(109),char(95),char(51),char(105),char(110),char(100),char(105),char(99),char(101), +char(115),char(56),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(109),char(95),char(110),char(117), +char(109),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(101),char(114),char(116), +char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(115),char(80),char(116),char(114), +char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(101),char(115),char(104), +char(80),char(97),char(114),char(116),char(115),char(0),char(109),char(95),char(109),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99), +char(101),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(70),char(108),char(111),char(97),char(116),char(66), +char(118),char(104),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(68),char(111),char(117),char(98),char(108), +char(101),char(66),char(118),char(104),char(0),char(42),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111), +char(77),char(97),char(112),char(0),char(109),char(95),char(112),char(97),char(100),char(51),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(114),char(105),char(109), +char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(116),char(114),char(97),char(110),char(115), +char(102),char(111),char(114),char(109),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(0),char(109), +char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104), +char(105),char(108),char(100),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104), +char(97),char(112),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(104),char(105),char(108),char(100),char(83),char(104),char(97), +char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115), +char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86),char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101), +char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86), +char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108), +char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101),char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118), +char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65), +char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105), +char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0), +char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114),char(116),char(101),char(120),char(84),char(104),char(114),char(101),char(115),char(104),char(111), +char(108),char(100),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(68),char(105),char(115),char(116),char(97),char(110),char(99),char(101),char(84),char(104),char(114), +char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(122),char(101),char(114),char(111),char(65),char(114),char(101),char(97),char(84),char(104),char(114), +char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95), +char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86), +char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(103),char(105), +char(109),char(112),char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99), +char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42), +char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98), +char(108),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80), +char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0), +char(42),char(109),char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104),char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0), +char(42),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(42),char(109), +char(95),char(114),char(111),char(111),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0), +char(109),char(95),char(119),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105), +char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97), +char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), +char(111),char(110),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105), +char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(86), +char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),char(105), +char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80), +char(114),char(111),char(99),char(101),char(115),char(115),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109), +char(95),char(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95), +char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(114),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114), +char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110), +char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(99),char(100), +char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114),char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(109),char(95), +char(99),char(99),char(100),char(77),char(111),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109), +char(95),char(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105),char(99),char(116), +char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),char(103),char(115), +char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84),char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112), +char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110), +char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112), +char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104), +char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118), +char(105),char(116),char(121),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99), +char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101), +char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(86),char(101), +char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111), +char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114), +char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114), +char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101),char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109), +char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116), +char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114), +char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101),char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95), +char(108),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117), +char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111), +char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(97), +char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105), +char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100), +char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110), +char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105), +char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103), +char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112), +char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), +char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0), +char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0), +char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0), +char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65),char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95), +char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111), +char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(100),char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101), +char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117), +char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68),char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95), +char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116), +char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100),char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111), +char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(116),char(101),char(114), +char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(98),char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112), +char(117),char(108),char(115),char(101),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(105),char(115),char(69),char(110), +char(97),char(98),char(108),char(101),char(100),char(0),char(109),char(95),char(116),char(121),char(112),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109),char(95), +char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101),char(0), +char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102),char(101), +char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97), +char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108),char(97), +char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),char(114),char(84),char(97),char(114),char(103),char(101),char(116), +char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(73), +char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0), +char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105),char(116), +char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116),char(111), +char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111),char(114), +char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110), +char(103),char(83),char(112),char(97),char(110),char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109), +char(95),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112), +char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101), +char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101), +char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101), +char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101), +char(102),char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79), +char(102),char(102),char(115),char(101),char(116),char(70),char(111),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(114), +char(97),char(109),char(101),char(0),char(109),char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114), +char(105),char(110),char(103),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105), +char(108),char(105),char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112), +char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115), +char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(116),char(97), +char(117),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),char(120),char(69),char(114), +char(114),char(111),char(114),char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),char(114),char(0),char(109), +char(95),char(101),char(114),char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),char(98),char(97),char(108), +char(67),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(80),char(101), +char(110),char(101),char(116),char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109), +char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),char(69),char(114),char(112), +char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),char(97),char(114),char(109), +char(115),char(116),char(97),char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(97),char(120), +char(71),char(121),char(114),char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(115),char(105), +char(110),char(103),char(108),char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116), +char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),char(109),char(73),char(116), +char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(77),char(111),char(100), +char(101),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(82),char(101), +char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109), +char(95),char(109),char(105),char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),char(99),char(104),char(83), +char(105),char(122),char(101),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109), +char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97), +char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(118),char(111), +char(108),char(117),char(109),char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(116), +char(101),char(114),char(105),char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112), +char(114),char(101),char(118),char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(118),char(101), +char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97),char(116),char(101),char(100), +char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95),char(97),char(114),char(101), +char(97),char(0),char(109),char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100), +char(105),char(99),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110),char(103),char(116),char(104), +char(0),char(109),char(95),char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110), +char(100),char(105),char(99),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114),char(101),char(97),char(0), +char(109),char(95),char(99),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101), +char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95), +char(99),char(49),char(0),char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108), +char(70),char(114),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(0),char(109), +char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111),char(77),char(111),char(100), +char(101),char(108),char(0),char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95),char(100),char(114),char(97), +char(103),char(0),char(109),char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117),char(114),char(101),char(0), +char(109),char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105),char(99),char(70),char(114), +char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99),char(104),char(0),char(109), +char(95),char(114),char(105),char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115), +char(115),char(0),char(109),char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97), +char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116), +char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(72),char(97),char(114), +char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117), +char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75), +char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115), +char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72), +char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67), +char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109), +char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73), +char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111), +char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105), +char(116),char(0),char(109),char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116),char(105),char(109),char(101), +char(83),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73),char(116),char(101),char(114), +char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(73),char(116),char(101), +char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116),char(101),char(114),char(97), +char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116),char(101),char(114),char(97), +char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(101),char(0), +char(109),char(95),char(97),char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(105),char(116), +char(105),char(111),char(110),char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109),char(95),char(110),char(117), +char(109),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87),char(101),char(105),char(103), +char(116),char(115),char(0),char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102),char(114),char(97),char(109), +char(101),char(0),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(108),char(111),char(99), +char(105),char(105),char(0),char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112),char(117),char(108),char(115), +char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93), +char(0),char(109),char(95),char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(114), +char(101),char(102),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(0),char(42), +char(109),char(95),char(109),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97),char(109),char(101),char(82), +char(101),char(102),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109), +char(77),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(105),char(109), +char(97),char(115),char(115),char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110), +char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112),char(105),char(110),char(103), +char(0),char(109),char(95),char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97),char(109),char(112),char(105), +char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(120),char(83), +char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0), +char(109),char(95),char(115),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108), +char(115),char(101),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105),char(110),char(115),char(65), +char(110),char(99),char(104),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109),char(95),char(99),char(108), +char(117),char(115),char(116),char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(0), +char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50),char(93),char(0),char(109), +char(95),char(99),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101),char(108),char(101),char(116), +char(101),char(0),char(109),char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50),char(93),char(0),char(109), +char(95),char(98),char(111),char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(116),char(121), +char(112),char(101),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(112),char(111), +char(115),char(101),char(0),char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(42),char(109),char(95), +char(110),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109),char(95),char(102),char(97), +char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(42),char(109), +char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(115), +char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(116),char(101), +char(114),char(105),char(97),char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0),char(109),char(95),char(110), +char(117),char(109),char(70),char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114),char(97),char(104),char(101), +char(100),char(114),char(97),char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(109),char(95),char(110), +char(117),char(109),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74),char(111),char(105),char(110), +char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(84),char(89),char(80),char(69),char(76),char(0),char(0),char(0), +char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104), +char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102), +char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105), +char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83), +char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99), +char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116), +char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114), +char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116), +char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84), +char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(70), +char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100), +char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97),char(116), +char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111), +char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99),char(80), +char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118), +char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115),char(0), +char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116), +char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112), +char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110),char(116), +char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108), +char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105), +char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83), +char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97), +char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97), +char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117), +char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110),char(100), +char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108), +char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108), +char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77), +char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101), +char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108), +char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116), +char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115), +char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), +char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108), +char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108), +char(100),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116), +char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49), +char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50), +char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108), +char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97), +char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103), +char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102), +char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110), +char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114), +char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121), +char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100), +char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(76),char(105), +char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97),char(99),char(101),char(68), +char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97),char(68),char(97),char(116), +char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114),char(68),char(97),char(116), +char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68),char(97),char(116),char(97), +char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111), +char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(0), +char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0), +char(8),char(0),char(0),char(0),char(12),char(0),char(36),char(0),char(8),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0), +char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(84),char(0),char(-124),char(0),char(12),char(0),char(52),char(0),char(52),char(0), +char(20),char(0),char(64),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),char(32),char(0),char(28),char(0),char(60),char(0),char(56),char(0), +char(76),char(0),char(76),char(0),char(24),char(0),char(60),char(0),char(60),char(0),char(16),char(0),char(64),char(0),char(68),char(0),char(-48),char(1),char(0),char(1), +char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-24),char(1),char(-96),char(3),char(8),char(0),char(52),char(0),char(0),char(0),char(84),char(0), +char(116),char(0),char(92),char(1),char(-36),char(0),char(-44),char(0),char(-4),char(0),char(92),char(1),char(-52),char(0),char(16),char(0),char(100),char(0),char(20),char(0), +char(36),char(0),char(100),char(0),char(92),char(0),char(104),char(0),char(-64),char(0),char(92),char(1),char(104),char(0),char(-84),char(1),char(83),char(84),char(82),char(67), +char(65),char(0),char(0),char(0),char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0), +char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0), +char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0), +char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0),char(13),char(0),char(9),char(0),char(16),char(0),char(1),char(0),char(14),char(0),char(9),char(0), +char(17),char(0),char(2),char(0),char(15),char(0),char(10),char(0),char(13),char(0),char(11),char(0),char(18),char(0),char(2),char(0),char(16),char(0),char(10),char(0), +char(14),char(0),char(11),char(0),char(19),char(0),char(4),char(0),char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0), +char(2),char(0),char(15),char(0),char(20),char(0),char(6),char(0),char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0), +char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(21),char(0),char(6),char(0),char(14),char(0),char(16),char(0), +char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0), +char(22),char(0),char(3),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(23),char(0),char(12),char(0), +char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0), +char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(20),char(0),char(30),char(0),char(22),char(0),char(31),char(0),char(19),char(0),char(32),char(0), +char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(24),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0), +char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0), +char(21),char(0),char(30),char(0),char(22),char(0),char(31),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(19),char(0),char(32),char(0), +char(25),char(0),char(3),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(26),char(0),char(5),char(0), +char(25),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0), +char(27),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0), +char(4),char(0),char(44),char(0),char(28),char(0),char(2),char(0),char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(29),char(0),char(4),char(0), +char(27),char(0),char(47),char(0),char(28),char(0),char(48),char(0),char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(30),char(0),char(1),char(0), +char(4),char(0),char(50),char(0),char(31),char(0),char(2),char(0),char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(32),char(0),char(2),char(0), +char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),char(33),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0), +char(34),char(0),char(8),char(0),char(13),char(0),char(54),char(0),char(14),char(0),char(55),char(0),char(30),char(0),char(56),char(0),char(32),char(0),char(57),char(0), +char(33),char(0),char(58),char(0),char(31),char(0),char(59),char(0),char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(35),char(0),char(4),char(0), +char(34),char(0),char(62),char(0),char(13),char(0),char(63),char(0),char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(36),char(0),char(7),char(0), +char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0),char(23),char(0),char(66),char(0),char(24),char(0),char(67),char(0),char(37),char(0),char(68),char(0), +char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),char(38),char(0),char(2),char(0),char(36),char(0),char(70),char(0),char(13),char(0),char(39),char(0), +char(39),char(0),char(4),char(0),char(17),char(0),char(71),char(0),char(25),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0), +char(40),char(0),char(4),char(0),char(25),char(0),char(38),char(0),char(39),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0), +char(41),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(42),char(0),char(3),char(0), +char(27),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(43),char(0),char(4),char(0),char(4),char(0),char(78),char(0), +char(7),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(37),char(0),char(14),char(0),char(4),char(0),char(82),char(0), +char(4),char(0),char(83),char(0),char(43),char(0),char(84),char(0),char(4),char(0),char(85),char(0),char(7),char(0),char(86),char(0),char(7),char(0),char(87),char(0), +char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(4),char(0),char(91),char(0),char(4),char(0),char(92),char(0), +char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(5),char(0),char(25),char(0),char(38),char(0), +char(35),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(95),char(0),char(45),char(0),char(5),char(0), +char(27),char(0),char(47),char(0),char(13),char(0),char(96),char(0),char(14),char(0),char(97),char(0),char(4),char(0),char(98),char(0),char(0),char(0),char(99),char(0), +char(46),char(0),char(25),char(0),char(9),char(0),char(100),char(0),char(9),char(0),char(101),char(0),char(25),char(0),char(102),char(0),char(0),char(0),char(35),char(0), +char(18),char(0),char(103),char(0),char(18),char(0),char(104),char(0),char(14),char(0),char(105),char(0),char(14),char(0),char(106),char(0),char(14),char(0),char(107),char(0), +char(8),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0),char(8),char(0),char(112),char(0), +char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(4),char(0),char(116),char(0),char(4),char(0),char(117),char(0), +char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0), +char(0),char(0),char(37),char(0),char(47),char(0),char(25),char(0),char(9),char(0),char(100),char(0),char(9),char(0),char(101),char(0),char(25),char(0),char(102),char(0), +char(0),char(0),char(35),char(0),char(17),char(0),char(103),char(0),char(17),char(0),char(104),char(0),char(13),char(0),char(105),char(0),char(13),char(0),char(106),char(0), +char(13),char(0),char(107),char(0),char(7),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0),char(7),char(0),char(111),char(0), +char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),char(4),char(0),char(116),char(0), +char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0), +char(4),char(0),char(122),char(0),char(0),char(0),char(37),char(0),char(48),char(0),char(2),char(0),char(49),char(0),char(123),char(0),char(14),char(0),char(124),char(0), +char(50),char(0),char(2),char(0),char(51),char(0),char(123),char(0),char(13),char(0),char(124),char(0),char(52),char(0),char(21),char(0),char(47),char(0),char(125),char(0), +char(15),char(0),char(126),char(0),char(13),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0),char(13),char(0),char(-126),char(0), +char(13),char(0),char(124),char(0),char(13),char(0),char(-125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0),char(13),char(0),char(-122),char(0), +char(7),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0),char(7),char(0),char(-117),char(0), +char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0),char(4),char(0),char(-112),char(0), +char(53),char(0),char(22),char(0),char(46),char(0),char(125),char(0),char(16),char(0),char(126),char(0),char(14),char(0),char(127),char(0),char(14),char(0),char(-128),char(0), +char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(124),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(-124),char(0), +char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(8),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0),char(8),char(0),char(-119),char(0), +char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0),char(8),char(0),char(-114),char(0), +char(8),char(0),char(-113),char(0),char(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(2),char(0),char(4),char(0),char(-111),char(0), +char(4),char(0),char(-110),char(0),char(55),char(0),char(13),char(0),char(56),char(0),char(-109),char(0),char(56),char(0),char(-108),char(0),char(0),char(0),char(35),char(0), +char(4),char(0),char(-107),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(7),char(0),char(-103),char(0), +char(7),char(0),char(-102),char(0),char(4),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(7),char(0),char(-99),char(0),char(4),char(0),char(-98),char(0), +char(57),char(0),char(3),char(0),char(55),char(0),char(-97),char(0),char(13),char(0),char(-96),char(0),char(13),char(0),char(-95),char(0),char(58),char(0),char(3),char(0), +char(55),char(0),char(-97),char(0),char(14),char(0),char(-96),char(0),char(14),char(0),char(-95),char(0),char(59),char(0),char(13),char(0),char(55),char(0),char(-97),char(0), +char(18),char(0),char(-94),char(0),char(18),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(4),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0), +char(7),char(0),char(-89),char(0),char(7),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0), +char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(60),char(0),char(13),char(0),char(55),char(0),char(-97),char(0),char(17),char(0),char(-94),char(0), +char(17),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(4),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(7),char(0),char(-89),char(0), +char(7),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0), +char(7),char(0),char(-83),char(0),char(61),char(0),char(11),char(0),char(55),char(0),char(-97),char(0),char(17),char(0),char(-94),char(0),char(17),char(0),char(-93),char(0), +char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0), +char(7),char(0),char(-83),char(0),char(7),char(0),char(-79),char(0),char(0),char(0),char(21),char(0),char(62),char(0),char(9),char(0),char(55),char(0),char(-97),char(0), +char(17),char(0),char(-94),char(0),char(17),char(0),char(-93),char(0),char(13),char(0),char(-78),char(0),char(13),char(0),char(-77),char(0),char(13),char(0),char(-76),char(0), +char(13),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),char(63),char(0),char(5),char(0),char(62),char(0),char(-72),char(0), +char(4),char(0),char(-71),char(0),char(7),char(0),char(-70),char(0),char(7),char(0),char(-69),char(0),char(7),char(0),char(-68),char(0),char(64),char(0),char(9),char(0), +char(55),char(0),char(-97),char(0),char(17),char(0),char(-94),char(0),char(17),char(0),char(-93),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0), +char(7),char(0),char(-76),char(0),char(7),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),char(49),char(0),char(22),char(0), +char(8),char(0),char(-67),char(0),char(8),char(0),char(-79),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(-66),char(0),char(8),char(0),char(112),char(0), +char(8),char(0),char(-65),char(0),char(8),char(0),char(-64),char(0),char(8),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(8),char(0),char(-61),char(0), +char(8),char(0),char(-60),char(0),char(8),char(0),char(-59),char(0),char(8),char(0),char(-58),char(0),char(8),char(0),char(-57),char(0),char(8),char(0),char(-56),char(0), +char(8),char(0),char(-55),char(0),char(4),char(0),char(-54),char(0),char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0), +char(4),char(0),char(-50),char(0),char(0),char(0),char(37),char(0),char(51),char(0),char(22),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-79),char(0), +char(7),char(0),char(110),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0), +char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0),char(7),char(0),char(-59),char(0), +char(7),char(0),char(-58),char(0),char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0),char(7),char(0),char(-55),char(0),char(4),char(0),char(-54),char(0), +char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(0),char(0),char(37),char(0), +char(65),char(0),char(4),char(0),char(7),char(0),char(-49),char(0),char(7),char(0),char(-48),char(0),char(7),char(0),char(-47),char(0),char(4),char(0),char(78),char(0), +char(66),char(0),char(10),char(0),char(65),char(0),char(-46),char(0),char(13),char(0),char(-45),char(0),char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0), +char(13),char(0),char(-42),char(0),char(13),char(0),char(-41),char(0),char(7),char(0),char(-121),char(0),char(7),char(0),char(-40),char(0),char(4),char(0),char(-39),char(0), +char(4),char(0),char(53),char(0),char(67),char(0),char(4),char(0),char(65),char(0),char(-46),char(0),char(4),char(0),char(-38),char(0),char(7),char(0),char(-37),char(0), +char(4),char(0),char(-36),char(0),char(68),char(0),char(4),char(0),char(13),char(0),char(-41),char(0),char(65),char(0),char(-46),char(0),char(4),char(0),char(-35),char(0), +char(7),char(0),char(-34),char(0),char(69),char(0),char(7),char(0),char(13),char(0),char(-33),char(0),char(65),char(0),char(-46),char(0),char(4),char(0),char(-32),char(0), +char(7),char(0),char(-31),char(0),char(7),char(0),char(-30),char(0),char(7),char(0),char(-29),char(0),char(4),char(0),char(53),char(0),char(70),char(0),char(6),char(0), +char(15),char(0),char(-28),char(0),char(13),char(0),char(-30),char(0),char(13),char(0),char(-27),char(0),char(56),char(0),char(-26),char(0),char(4),char(0),char(-25),char(0), +char(7),char(0),char(-29),char(0),char(71),char(0),char(26),char(0),char(4),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-79),char(0), +char(7),char(0),char(-22),char(0),char(7),char(0),char(-21),char(0),char(7),char(0),char(-20),char(0),char(7),char(0),char(-19),char(0),char(7),char(0),char(-18),char(0), +char(7),char(0),char(-17),char(0),char(7),char(0),char(-16),char(0),char(7),char(0),char(-15),char(0),char(7),char(0),char(-14),char(0),char(7),char(0),char(-13),char(0), +char(7),char(0),char(-12),char(0),char(7),char(0),char(-11),char(0),char(7),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0),char(7),char(0),char(-8),char(0), +char(7),char(0),char(-7),char(0),char(7),char(0),char(-6),char(0),char(7),char(0),char(-5),char(0),char(4),char(0),char(-4),char(0),char(4),char(0),char(-3),char(0), +char(4),char(0),char(-2),char(0),char(4),char(0),char(-1),char(0),char(4),char(0),char(117),char(0),char(72),char(0),char(12),char(0),char(15),char(0),char(0),char(1), +char(15),char(0),char(1),char(1),char(15),char(0),char(2),char(1),char(13),char(0),char(3),char(1),char(13),char(0),char(4),char(1),char(7),char(0),char(5),char(1), +char(4),char(0),char(6),char(1),char(4),char(0),char(7),char(1),char(4),char(0),char(8),char(1),char(4),char(0),char(9),char(1),char(7),char(0),char(-31),char(0), +char(4),char(0),char(53),char(0),char(73),char(0),char(27),char(0),char(17),char(0),char(10),char(1),char(15),char(0),char(11),char(1),char(15),char(0),char(12),char(1), +char(13),char(0),char(3),char(1),char(13),char(0),char(13),char(1),char(13),char(0),char(14),char(1),char(13),char(0),char(15),char(1),char(13),char(0),char(16),char(1), +char(13),char(0),char(17),char(1),char(4),char(0),char(18),char(1),char(7),char(0),char(19),char(1),char(4),char(0),char(20),char(1),char(4),char(0),char(21),char(1), +char(4),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1),char(4),char(0),char(25),char(1),char(4),char(0),char(26),char(1), +char(7),char(0),char(27),char(1),char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1),char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1), +char(7),char(0),char(32),char(1),char(4),char(0),char(33),char(1),char(4),char(0),char(34),char(1),char(4),char(0),char(35),char(1),char(74),char(0),char(12),char(0), +char(9),char(0),char(36),char(1),char(9),char(0),char(37),char(1),char(13),char(0),char(38),char(1),char(7),char(0),char(39),char(1),char(7),char(0),char(-63),char(0), +char(7),char(0),char(40),char(1),char(4),char(0),char(41),char(1),char(13),char(0),char(42),char(1),char(4),char(0),char(43),char(1),char(4),char(0),char(44),char(1), +char(4),char(0),char(45),char(1),char(4),char(0),char(53),char(0),char(75),char(0),char(19),char(0),char(47),char(0),char(125),char(0),char(72),char(0),char(46),char(1), +char(65),char(0),char(47),char(1),char(66),char(0),char(48),char(1),char(67),char(0),char(49),char(1),char(68),char(0),char(50),char(1),char(69),char(0),char(51),char(1), +char(70),char(0),char(52),char(1),char(73),char(0),char(53),char(1),char(74),char(0),char(54),char(1),char(4),char(0),char(55),char(1),char(4),char(0),char(21),char(1), +char(4),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(4),char(0),char(58),char(1),char(4),char(0),char(59),char(1),char(4),char(0),char(60),char(1), +char(4),char(0),char(61),char(1),char(71),char(0),char(62),char(1),}; +int b3s_bulletDNAlen= sizeof(b3s_bulletDNAstr); +char b3s_bulletDNAstr64[]= { +char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(63),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), +char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95), +char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111), +char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(115),char(0),char(42),char(102),char(105),char(114),char(115),char(116),char(0),char(42),char(108),char(97),char(115), +char(116),char(0),char(109),char(95),char(102),char(108),char(111),char(97),char(116),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(108),char(91),char(51), +char(93),char(0),char(109),char(95),char(98),char(97),char(115),char(105),char(115),char(0),char(109),char(95),char(111),char(114),char(105),char(103),char(105),char(110),char(0),char(109), +char(95),char(114),char(111),char(111),char(116),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98), +char(116),char(114),char(101),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100), +char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(91),char(51),char(93),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122), +char(101),char(100),char(65),char(97),char(98),char(98),char(77),char(97),char(120),char(91),char(51),char(93),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77), +char(105),char(110),char(79),char(114),char(103),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77),char(97),char(120),char(79),char(114),char(103),char(0),char(109), +char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98),char(80),char(97), +char(114),char(116),char(0),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109), +char(95),char(112),char(97),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101), +char(120),char(79),char(114),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(98), +char(118),char(104),char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(0),char(109),char(95),char(98),char(118),char(104),char(65),char(97),char(98),char(98),char(77), +char(97),char(120),char(0),char(109),char(95),char(98),char(118),char(104),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110), +char(0),char(109),char(95),char(99),char(117),char(114),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(117),char(115), +char(101),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(110),char(117),char(109),char(67), +char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(76),char(101),char(97),char(102),char(78),char(111),char(100),char(101),char(115),char(0),char(109), +char(95),char(110),char(117),char(109),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117), +char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(99),char(111),char(110),char(116),char(105),char(103),char(117),char(111), +char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105), +char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116), +char(114),char(0),char(42),char(109),char(95),char(115),char(117),char(98),char(84),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(80),char(116),char(114),char(0), +char(109),char(95),char(116),char(114),char(97),char(118),char(101),char(114),char(115),char(97),char(108),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(110),char(117), +char(109),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(72),char(101),char(97),char(100),char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(110), +char(97),char(109),char(101),char(0),char(109),char(95),char(115),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(112),char(97), +char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110), +char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(83),char(99),char(97), +char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(101),char(78),char(111),char(114),char(109),char(97),char(108),char(0),char(109), +char(95),char(112),char(108),char(97),char(110),char(101),char(67),char(111),char(110),char(115),char(116),char(97),char(110),char(116),char(0),char(109),char(95),char(105),char(109),char(112), +char(108),char(105),char(99),char(105),char(116),char(83),char(104),char(97),char(112),char(101),char(68),char(105),char(109),char(101),char(110),char(115),char(105),char(111),char(110),char(115), +char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(109), +char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(111),char(115),char(0),char(109),char(95),char(114),char(97),char(100), +char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108), +char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(42),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(80),char(111), +char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(108),char(111),char(99), +char(97),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(83),char(105),char(122),char(101),char(0), +char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(0),char(109),char(95),char(112),char(97),char(100),char(91),char(50),char(93),char(0),char(109),char(95),char(118), +char(97),char(108),char(117),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(112),char(97),char(100),char(0),char(42),char(109),char(95),char(118),char(101), +char(114),char(116),char(105),char(99),char(101),char(115),char(51),char(102),char(0),char(42),char(109),char(95),char(118),char(101),char(114),char(116),char(105),char(99),char(101),char(115), +char(51),char(100),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(51),char(50),char(0),char(42),char(109),char(95),char(51), +char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(42),char(109),char(95),char(51),char(105),char(110),char(100),char(105),char(99),char(101), +char(115),char(56),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(109),char(95),char(110),char(117), +char(109),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(101),char(114),char(116), +char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(115),char(80),char(116),char(114), +char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(101),char(115),char(104), +char(80),char(97),char(114),char(116),char(115),char(0),char(109),char(95),char(109),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99), +char(101),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(70),char(108),char(111),char(97),char(116),char(66), +char(118),char(104),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(68),char(111),char(117),char(98),char(108), +char(101),char(66),char(118),char(104),char(0),char(42),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111), +char(77),char(97),char(112),char(0),char(109),char(95),char(112),char(97),char(100),char(51),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(114),char(105),char(109), +char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(116),char(114),char(97),char(110),char(115), +char(102),char(111),char(114),char(109),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(0),char(109), +char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104), +char(105),char(108),char(100),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104), +char(97),char(112),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(104),char(105),char(108),char(100),char(83),char(104),char(97), +char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115), +char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86),char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101), +char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86), +char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108), +char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101),char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118), +char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65), +char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105), +char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0), +char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114),char(116),char(101),char(120),char(84),char(104),char(114),char(101),char(115),char(104),char(111), +char(108),char(100),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(68),char(105),char(115),char(116),char(97),char(110),char(99),char(101),char(84),char(104),char(114), +char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(122),char(101),char(114),char(111),char(65),char(114),char(101),char(97),char(84),char(104),char(114), +char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95), +char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86), +char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(103),char(105), +char(109),char(112),char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99), +char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42), +char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98), +char(108),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80), +char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0), +char(42),char(109),char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104),char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0), +char(42),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(42),char(109), +char(95),char(114),char(111),char(111),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0), +char(109),char(95),char(119),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105), +char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97), +char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), +char(111),char(110),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105), +char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(86), +char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),char(105), +char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80), +char(114),char(111),char(99),char(101),char(115),char(115),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109), +char(95),char(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95), +char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(114),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114), +char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110), +char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(99),char(100), +char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114),char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(109),char(95), +char(99),char(99),char(100),char(77),char(111),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109), +char(95),char(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105),char(99),char(116), +char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),char(103),char(115), +char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84),char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112), +char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110), +char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112), +char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104), +char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118), +char(105),char(116),char(121),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99), +char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101), +char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(86),char(101), +char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111), +char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114), +char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114), +char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101),char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109), +char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116), +char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114), +char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101),char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95), +char(108),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117), +char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111), +char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(97), +char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105), +char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100), +char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110), +char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105), +char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103), +char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112), +char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), +char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0), +char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0), +char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0), +char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65),char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95), +char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111), +char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(100),char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101), +char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117), +char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68),char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95), +char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116), +char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100),char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111), +char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(116),char(101),char(114), +char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(98),char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112), +char(117),char(108),char(115),char(101),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(105),char(115),char(69),char(110), +char(97),char(98),char(108),char(101),char(100),char(0),char(109),char(95),char(116),char(121),char(112),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109),char(95), +char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101),char(0), +char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102),char(101), +char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97), +char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108),char(97), +char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),char(114),char(84),char(97),char(114),char(103),char(101),char(116), +char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(73), +char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0), +char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105),char(116), +char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116),char(111), +char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111),char(114), +char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110), +char(103),char(83),char(112),char(97),char(110),char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109), +char(95),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112), +char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101), +char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101), +char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101), +char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101), +char(102),char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79), +char(102),char(102),char(115),char(101),char(116),char(70),char(111),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(114), +char(97),char(109),char(101),char(0),char(109),char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114), +char(105),char(110),char(103),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105), +char(108),char(105),char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112), +char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115), +char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(116),char(97), +char(117),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),char(120),char(69),char(114), +char(114),char(111),char(114),char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),char(114),char(0),char(109), +char(95),char(101),char(114),char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),char(98),char(97),char(108), +char(67),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(80),char(101), +char(110),char(101),char(116),char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109), +char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),char(69),char(114),char(112), +char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),char(97),char(114),char(109), +char(115),char(116),char(97),char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(97),char(120), +char(71),char(121),char(114),char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(115),char(105), +char(110),char(103),char(108),char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116), +char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),char(109),char(73),char(116), +char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(77),char(111),char(100), +char(101),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(82),char(101), +char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109), +char(95),char(109),char(105),char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),char(99),char(104),char(83), +char(105),char(122),char(101),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109), +char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97), +char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(118),char(111), +char(108),char(117),char(109),char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(116), +char(101),char(114),char(105),char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112), +char(114),char(101),char(118),char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(118),char(101), +char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97),char(116),char(101),char(100), +char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95),char(97),char(114),char(101), +char(97),char(0),char(109),char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100), +char(105),char(99),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110),char(103),char(116),char(104), +char(0),char(109),char(95),char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110), +char(100),char(105),char(99),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114),char(101),char(97),char(0), +char(109),char(95),char(99),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101), +char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95), +char(99),char(49),char(0),char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108), +char(70),char(114),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(0),char(109), +char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111),char(77),char(111),char(100), +char(101),char(108),char(0),char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95),char(100),char(114),char(97), +char(103),char(0),char(109),char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117),char(114),char(101),char(0), +char(109),char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105),char(99),char(70),char(114), +char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99),char(104),char(0),char(109), +char(95),char(114),char(105),char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115), +char(115),char(0),char(109),char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97), +char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116), +char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(72),char(97),char(114), +char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117), +char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75), +char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115), +char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72), +char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67), +char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109), +char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73), +char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111), +char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105), +char(116),char(0),char(109),char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116),char(105),char(109),char(101), +char(83),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73),char(116),char(101),char(114), +char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(73),char(116),char(101), +char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116),char(101),char(114),char(97), +char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116),char(101),char(114),char(97), +char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(101),char(0), +char(109),char(95),char(97),char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(105),char(116), +char(105),char(111),char(110),char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109),char(95),char(110),char(117), +char(109),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87),char(101),char(105),char(103), +char(116),char(115),char(0),char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102),char(114),char(97),char(109), +char(101),char(0),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(108),char(111),char(99), +char(105),char(105),char(0),char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112),char(117),char(108),char(115), +char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93), +char(0),char(109),char(95),char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(114), +char(101),char(102),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(0),char(42), +char(109),char(95),char(109),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97),char(109),char(101),char(82), +char(101),char(102),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109), +char(77),char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(105),char(109), +char(97),char(115),char(115),char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110), +char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112),char(105),char(110),char(103), +char(0),char(109),char(95),char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97),char(109),char(112),char(105), +char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(120),char(83), +char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0), +char(109),char(95),char(115),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108), +char(115),char(101),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105),char(110),char(115),char(65), +char(110),char(99),char(104),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109),char(95),char(99),char(108), +char(117),char(115),char(116),char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(0), +char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50),char(93),char(0),char(109), +char(95),char(99),char(102),char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101),char(108),char(101),char(116), +char(101),char(0),char(109),char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50),char(93),char(0),char(109), +char(95),char(98),char(111),char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(116),char(121), +char(112),char(101),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(112),char(111), +char(115),char(101),char(0),char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(42),char(109),char(95), +char(110),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109),char(95),char(102),char(97), +char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(42),char(109), +char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(115), +char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(116),char(101), +char(114),char(105),char(97),char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0),char(109),char(95),char(110), +char(117),char(109),char(70),char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114),char(97),char(104),char(101), +char(100),char(114),char(97),char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(109),char(95),char(110), +char(117),char(109),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74),char(111),char(105),char(110), +char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(84),char(89),char(80),char(69),char(76),char(0),char(0),char(0), +char(99),char(104),char(97),char(114),char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104), +char(111),char(114),char(116),char(0),char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102), +char(108),char(111),char(97),char(116),char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105), +char(110),char(116),char(101),char(114),char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83), +char(121),char(115),char(116),char(101),char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99), +char(116),char(111),char(114),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116), +char(111),char(114),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116),char(114), +char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116), +char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84), +char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(70), +char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100), +char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97),char(116), +char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111), +char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99),char(80), +char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118), +char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115),char(0), +char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101),char(116), +char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112), +char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110),char(116), +char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108), +char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105), +char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83), +char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97), +char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104),char(97), +char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117), +char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110),char(100), +char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108), +char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108), +char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77), +char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101), +char(120),char(72),char(117),char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108), +char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116), +char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115), +char(87),char(111),char(114),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), +char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108), +char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108), +char(100),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116), +char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0), +char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49), +char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50), +char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108), +char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97), +char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103), +char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102), +char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110), +char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114), +char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121), +char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100), +char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(76),char(105), +char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97),char(99),char(101),char(68), +char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97),char(68),char(97),char(116), +char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114),char(68),char(97),char(116), +char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68),char(97),char(116),char(97), +char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111), +char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(0), +char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0), +char(8),char(0),char(0),char(0),char(16),char(0),char(48),char(0),char(16),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0), +char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(96),char(0),char(-112),char(0),char(16),char(0),char(56),char(0),char(56),char(0), +char(20),char(0),char(72),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),char(56),char(0),char(32),char(0),char(80),char(0),char(72),char(0), +char(96),char(0),char(80),char(0),char(32),char(0),char(64),char(0),char(64),char(0),char(16),char(0),char(72),char(0),char(80),char(0),char(-32),char(1),char(16),char(1), +char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-8),char(1),char(-80),char(3),char(8),char(0),char(64),char(0),char(0),char(0),char(96),char(0), +char(-128),char(0),char(104),char(1),char(-24),char(0),char(-32),char(0),char(8),char(1),char(104),char(1),char(-40),char(0),char(16),char(0),char(104),char(0),char(24),char(0), +char(40),char(0),char(104),char(0),char(96),char(0),char(104),char(0),char(-56),char(0),char(104),char(1),char(112),char(0),char(-32),char(1),char(83),char(84),char(82),char(67), +char(65),char(0),char(0),char(0),char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0), +char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0), +char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0), +char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0),char(13),char(0),char(9),char(0),char(16),char(0),char(1),char(0),char(14),char(0),char(9),char(0), +char(17),char(0),char(2),char(0),char(15),char(0),char(10),char(0),char(13),char(0),char(11),char(0),char(18),char(0),char(2),char(0),char(16),char(0),char(10),char(0), +char(14),char(0),char(11),char(0),char(19),char(0),char(4),char(0),char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0), +char(2),char(0),char(15),char(0),char(20),char(0),char(6),char(0),char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0), +char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(21),char(0),char(6),char(0),char(14),char(0),char(16),char(0), +char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0), +char(22),char(0),char(3),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(23),char(0),char(12),char(0), +char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0), +char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(20),char(0),char(30),char(0),char(22),char(0),char(31),char(0),char(19),char(0),char(32),char(0), +char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(24),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0), +char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0), +char(21),char(0),char(30),char(0),char(22),char(0),char(31),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(19),char(0),char(32),char(0), +char(25),char(0),char(3),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(26),char(0),char(5),char(0), +char(25),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0), +char(27),char(0),char(5),char(0),char(25),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0), +char(4),char(0),char(44),char(0),char(28),char(0),char(2),char(0),char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(29),char(0),char(4),char(0), +char(27),char(0),char(47),char(0),char(28),char(0),char(48),char(0),char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(30),char(0),char(1),char(0), +char(4),char(0),char(50),char(0),char(31),char(0),char(2),char(0),char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(32),char(0),char(2),char(0), +char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),char(33),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0), +char(34),char(0),char(8),char(0),char(13),char(0),char(54),char(0),char(14),char(0),char(55),char(0),char(30),char(0),char(56),char(0),char(32),char(0),char(57),char(0), +char(33),char(0),char(58),char(0),char(31),char(0),char(59),char(0),char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(35),char(0),char(4),char(0), +char(34),char(0),char(62),char(0),char(13),char(0),char(63),char(0),char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(36),char(0),char(7),char(0), +char(25),char(0),char(38),char(0),char(35),char(0),char(65),char(0),char(23),char(0),char(66),char(0),char(24),char(0),char(67),char(0),char(37),char(0),char(68),char(0), +char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),char(38),char(0),char(2),char(0),char(36),char(0),char(70),char(0),char(13),char(0),char(39),char(0), +char(39),char(0),char(4),char(0),char(17),char(0),char(71),char(0),char(25),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0), +char(40),char(0),char(4),char(0),char(25),char(0),char(38),char(0),char(39),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0), +char(41),char(0),char(3),char(0),char(27),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(42),char(0),char(3),char(0), +char(27),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(43),char(0),char(4),char(0),char(4),char(0),char(78),char(0), +char(7),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(37),char(0),char(14),char(0),char(4),char(0),char(82),char(0), +char(4),char(0),char(83),char(0),char(43),char(0),char(84),char(0),char(4),char(0),char(85),char(0),char(7),char(0),char(86),char(0),char(7),char(0),char(87),char(0), +char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(4),char(0),char(91),char(0),char(4),char(0),char(92),char(0), +char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(5),char(0),char(25),char(0),char(38),char(0), +char(35),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(95),char(0),char(45),char(0),char(5),char(0), +char(27),char(0),char(47),char(0),char(13),char(0),char(96),char(0),char(14),char(0),char(97),char(0),char(4),char(0),char(98),char(0),char(0),char(0),char(99),char(0), +char(46),char(0),char(25),char(0),char(9),char(0),char(100),char(0),char(9),char(0),char(101),char(0),char(25),char(0),char(102),char(0),char(0),char(0),char(35),char(0), +char(18),char(0),char(103),char(0),char(18),char(0),char(104),char(0),char(14),char(0),char(105),char(0),char(14),char(0),char(106),char(0),char(14),char(0),char(107),char(0), +char(8),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0),char(8),char(0),char(112),char(0), +char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(4),char(0),char(116),char(0),char(4),char(0),char(117),char(0), +char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0), +char(0),char(0),char(37),char(0),char(47),char(0),char(25),char(0),char(9),char(0),char(100),char(0),char(9),char(0),char(101),char(0),char(25),char(0),char(102),char(0), +char(0),char(0),char(35),char(0),char(17),char(0),char(103),char(0),char(17),char(0),char(104),char(0),char(13),char(0),char(105),char(0),char(13),char(0),char(106),char(0), +char(13),char(0),char(107),char(0),char(7),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0),char(7),char(0),char(111),char(0), +char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),char(4),char(0),char(116),char(0), +char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0), +char(4),char(0),char(122),char(0),char(0),char(0),char(37),char(0),char(48),char(0),char(2),char(0),char(49),char(0),char(123),char(0),char(14),char(0),char(124),char(0), +char(50),char(0),char(2),char(0),char(51),char(0),char(123),char(0),char(13),char(0),char(124),char(0),char(52),char(0),char(21),char(0),char(47),char(0),char(125),char(0), +char(15),char(0),char(126),char(0),char(13),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0),char(13),char(0),char(-126),char(0), +char(13),char(0),char(124),char(0),char(13),char(0),char(-125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0),char(13),char(0),char(-122),char(0), +char(7),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0),char(7),char(0),char(-117),char(0), +char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0),char(4),char(0),char(-112),char(0), +char(53),char(0),char(22),char(0),char(46),char(0),char(125),char(0),char(16),char(0),char(126),char(0),char(14),char(0),char(127),char(0),char(14),char(0),char(-128),char(0), +char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(124),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(-124),char(0), +char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(8),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0),char(8),char(0),char(-119),char(0), +char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0),char(8),char(0),char(-114),char(0), +char(8),char(0),char(-113),char(0),char(4),char(0),char(-112),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(2),char(0),char(4),char(0),char(-111),char(0), +char(4),char(0),char(-110),char(0),char(55),char(0),char(13),char(0),char(56),char(0),char(-109),char(0),char(56),char(0),char(-108),char(0),char(0),char(0),char(35),char(0), +char(4),char(0),char(-107),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(7),char(0),char(-103),char(0), +char(7),char(0),char(-102),char(0),char(4),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(7),char(0),char(-99),char(0),char(4),char(0),char(-98),char(0), +char(57),char(0),char(3),char(0),char(55),char(0),char(-97),char(0),char(13),char(0),char(-96),char(0),char(13),char(0),char(-95),char(0),char(58),char(0),char(3),char(0), +char(55),char(0),char(-97),char(0),char(14),char(0),char(-96),char(0),char(14),char(0),char(-95),char(0),char(59),char(0),char(13),char(0),char(55),char(0),char(-97),char(0), +char(18),char(0),char(-94),char(0),char(18),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(4),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0), +char(7),char(0),char(-89),char(0),char(7),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0), +char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(60),char(0),char(13),char(0),char(55),char(0),char(-97),char(0),char(17),char(0),char(-94),char(0), +char(17),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(4),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(7),char(0),char(-89),char(0), +char(7),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0), +char(7),char(0),char(-83),char(0),char(61),char(0),char(11),char(0),char(55),char(0),char(-97),char(0),char(17),char(0),char(-94),char(0),char(17),char(0),char(-93),char(0), +char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0), +char(7),char(0),char(-83),char(0),char(7),char(0),char(-79),char(0),char(0),char(0),char(21),char(0),char(62),char(0),char(9),char(0),char(55),char(0),char(-97),char(0), +char(17),char(0),char(-94),char(0),char(17),char(0),char(-93),char(0),char(13),char(0),char(-78),char(0),char(13),char(0),char(-77),char(0),char(13),char(0),char(-76),char(0), +char(13),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),char(63),char(0),char(5),char(0),char(62),char(0),char(-72),char(0), +char(4),char(0),char(-71),char(0),char(7),char(0),char(-70),char(0),char(7),char(0),char(-69),char(0),char(7),char(0),char(-68),char(0),char(64),char(0),char(9),char(0), +char(55),char(0),char(-97),char(0),char(17),char(0),char(-94),char(0),char(17),char(0),char(-93),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0), +char(7),char(0),char(-76),char(0),char(7),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),char(49),char(0),char(22),char(0), +char(8),char(0),char(-67),char(0),char(8),char(0),char(-79),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(-66),char(0),char(8),char(0),char(112),char(0), +char(8),char(0),char(-65),char(0),char(8),char(0),char(-64),char(0),char(8),char(0),char(-63),char(0),char(8),char(0),char(-62),char(0),char(8),char(0),char(-61),char(0), +char(8),char(0),char(-60),char(0),char(8),char(0),char(-59),char(0),char(8),char(0),char(-58),char(0),char(8),char(0),char(-57),char(0),char(8),char(0),char(-56),char(0), +char(8),char(0),char(-55),char(0),char(4),char(0),char(-54),char(0),char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0), +char(4),char(0),char(-50),char(0),char(0),char(0),char(37),char(0),char(51),char(0),char(22),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-79),char(0), +char(7),char(0),char(110),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(-65),char(0),char(7),char(0),char(-64),char(0), +char(7),char(0),char(-63),char(0),char(7),char(0),char(-62),char(0),char(7),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0),char(7),char(0),char(-59),char(0), +char(7),char(0),char(-58),char(0),char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0),char(7),char(0),char(-55),char(0),char(4),char(0),char(-54),char(0), +char(4),char(0),char(-53),char(0),char(4),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(4),char(0),char(-50),char(0),char(0),char(0),char(37),char(0), +char(65),char(0),char(4),char(0),char(7),char(0),char(-49),char(0),char(7),char(0),char(-48),char(0),char(7),char(0),char(-47),char(0),char(4),char(0),char(78),char(0), +char(66),char(0),char(10),char(0),char(65),char(0),char(-46),char(0),char(13),char(0),char(-45),char(0),char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0), +char(13),char(0),char(-42),char(0),char(13),char(0),char(-41),char(0),char(7),char(0),char(-121),char(0),char(7),char(0),char(-40),char(0),char(4),char(0),char(-39),char(0), +char(4),char(0),char(53),char(0),char(67),char(0),char(4),char(0),char(65),char(0),char(-46),char(0),char(4),char(0),char(-38),char(0),char(7),char(0),char(-37),char(0), +char(4),char(0),char(-36),char(0),char(68),char(0),char(4),char(0),char(13),char(0),char(-41),char(0),char(65),char(0),char(-46),char(0),char(4),char(0),char(-35),char(0), +char(7),char(0),char(-34),char(0),char(69),char(0),char(7),char(0),char(13),char(0),char(-33),char(0),char(65),char(0),char(-46),char(0),char(4),char(0),char(-32),char(0), +char(7),char(0),char(-31),char(0),char(7),char(0),char(-30),char(0),char(7),char(0),char(-29),char(0),char(4),char(0),char(53),char(0),char(70),char(0),char(6),char(0), +char(15),char(0),char(-28),char(0),char(13),char(0),char(-30),char(0),char(13),char(0),char(-27),char(0),char(56),char(0),char(-26),char(0),char(4),char(0),char(-25),char(0), +char(7),char(0),char(-29),char(0),char(71),char(0),char(26),char(0),char(4),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-79),char(0), +char(7),char(0),char(-22),char(0),char(7),char(0),char(-21),char(0),char(7),char(0),char(-20),char(0),char(7),char(0),char(-19),char(0),char(7),char(0),char(-18),char(0), +char(7),char(0),char(-17),char(0),char(7),char(0),char(-16),char(0),char(7),char(0),char(-15),char(0),char(7),char(0),char(-14),char(0),char(7),char(0),char(-13),char(0), +char(7),char(0),char(-12),char(0),char(7),char(0),char(-11),char(0),char(7),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0),char(7),char(0),char(-8),char(0), +char(7),char(0),char(-7),char(0),char(7),char(0),char(-6),char(0),char(7),char(0),char(-5),char(0),char(4),char(0),char(-4),char(0),char(4),char(0),char(-3),char(0), +char(4),char(0),char(-2),char(0),char(4),char(0),char(-1),char(0),char(4),char(0),char(117),char(0),char(72),char(0),char(12),char(0),char(15),char(0),char(0),char(1), +char(15),char(0),char(1),char(1),char(15),char(0),char(2),char(1),char(13),char(0),char(3),char(1),char(13),char(0),char(4),char(1),char(7),char(0),char(5),char(1), +char(4),char(0),char(6),char(1),char(4),char(0),char(7),char(1),char(4),char(0),char(8),char(1),char(4),char(0),char(9),char(1),char(7),char(0),char(-31),char(0), +char(4),char(0),char(53),char(0),char(73),char(0),char(27),char(0),char(17),char(0),char(10),char(1),char(15),char(0),char(11),char(1),char(15),char(0),char(12),char(1), +char(13),char(0),char(3),char(1),char(13),char(0),char(13),char(1),char(13),char(0),char(14),char(1),char(13),char(0),char(15),char(1),char(13),char(0),char(16),char(1), +char(13),char(0),char(17),char(1),char(4),char(0),char(18),char(1),char(7),char(0),char(19),char(1),char(4),char(0),char(20),char(1),char(4),char(0),char(21),char(1), +char(4),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1),char(4),char(0),char(25),char(1),char(4),char(0),char(26),char(1), +char(7),char(0),char(27),char(1),char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1),char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1), +char(7),char(0),char(32),char(1),char(4),char(0),char(33),char(1),char(4),char(0),char(34),char(1),char(4),char(0),char(35),char(1),char(74),char(0),char(12),char(0), +char(9),char(0),char(36),char(1),char(9),char(0),char(37),char(1),char(13),char(0),char(38),char(1),char(7),char(0),char(39),char(1),char(7),char(0),char(-63),char(0), +char(7),char(0),char(40),char(1),char(4),char(0),char(41),char(1),char(13),char(0),char(42),char(1),char(4),char(0),char(43),char(1),char(4),char(0),char(44),char(1), +char(4),char(0),char(45),char(1),char(4),char(0),char(53),char(0),char(75),char(0),char(19),char(0),char(47),char(0),char(125),char(0),char(72),char(0),char(46),char(1), +char(65),char(0),char(47),char(1),char(66),char(0),char(48),char(1),char(67),char(0),char(49),char(1),char(68),char(0),char(50),char(1),char(69),char(0),char(51),char(1), +char(70),char(0),char(52),char(1),char(73),char(0),char(53),char(1),char(74),char(0),char(54),char(1),char(4),char(0),char(55),char(1),char(4),char(0),char(21),char(1), +char(4),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(4),char(0),char(58),char(1),char(4),char(0),char(59),char(1),char(4),char(0),char(60),char(1), +char(4),char(0),char(61),char(1),char(71),char(0),char(62),char(1),}; +int b3s_bulletDNAlen64= sizeof(b3s_bulletDNAstr64); diff --git a/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.h b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.h new file mode 100644 index 000000000000..1c1ce4376473 --- /dev/null +++ b/extern/bullet/src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.h @@ -0,0 +1,639 @@ +/* +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. +*/ + +#ifndef B3_SERIALIZER_H +#define B3_SERIALIZER_H + +#include "Bullet3Common/b3Scalar.h" // has definitions like B3_FORCE_INLINE +#include "Bullet3Common/b3StackAlloc.h" +#include "Bullet3Common/b3HashMap.h" + +#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) +#include +#endif +#include + + + +extern char b3s_bulletDNAstr[]; +extern int b3s_bulletDNAlen; +extern char b3s_bulletDNAstr64[]; +extern int b3s_bulletDNAlen64; + +B3_FORCE_INLINE int b3StrLen(const char* str) +{ + if (!str) + return(0); + int len = 0; + + while (*str != 0) + { + str++; + len++; + } + + return len; +} + + +class b3Chunk +{ +public: + int m_chunkCode; + int m_length; + void *m_oldPtr; + int m_dna_nr; + int m_number; +}; + +enum b3SerializationFlags +{ + B3_SERIALIZE_NO_BVH = 1, + B3_SERIALIZE_NO_TRIANGLEINFOMAP = 2, + B3_SERIALIZE_NO_DUPLICATE_ASSERT = 4 +}; + +class b3Serializer +{ + +public: + + virtual ~b3Serializer() {} + + virtual const unsigned char* getBufferPointer() const = 0; + + virtual int getCurrentBufferSize() const = 0; + + virtual b3Chunk* allocate(size_t size, int numElements) = 0; + + virtual void finalizeChunk(b3Chunk* chunk, const char* structType, int chunkCode,void* oldPtr)= 0; + + virtual void* findPointer(void* oldPtr) = 0; + + virtual void* getUniquePointer(void*oldPtr) = 0; + + virtual void startSerialization() = 0; + + virtual void finishSerialization() = 0; + + virtual const char* findNameForPointer(const void* ptr) const = 0; + + virtual void registerNameForPointer(const void* ptr, const char* name) = 0; + + virtual void serializeName(const char* ptr) = 0; + + virtual int getSerializationFlags() const = 0; + + virtual void setSerializationFlags(int flags) = 0; + + +}; + + + +#define B3_HEADER_LENGTH 12 +#if defined(__sgi) || defined (__sparc) || defined (__sparc__) || defined (__PPC__) || defined (__ppc__) || defined (__BIG_ENDIAN__) +# define B3_MAKE_ID(a,b,c,d) ( (int)(a)<<24 | (int)(b)<<16 | (c)<<8 | (d) ) +#else +# define B3_MAKE_ID(a,b,c,d) ( (int)(d)<<24 | (int)(c)<<16 | (b)<<8 | (a) ) +#endif + +#define B3_SOFTBODY_CODE B3_MAKE_ID('S','B','D','Y') +#define B3_COLLISIONOBJECT_CODE B3_MAKE_ID('C','O','B','J') +#define B3_RIGIDBODY_CODE B3_MAKE_ID('R','B','D','Y') +#define B3_CONSTRAINT_CODE B3_MAKE_ID('C','O','N','S') +#define B3_BOXSHAPE_CODE B3_MAKE_ID('B','O','X','S') +#define B3_QUANTIZED_BVH_CODE B3_MAKE_ID('Q','B','V','H') +#define B3_TRIANLGE_INFO_MAP B3_MAKE_ID('T','M','A','P') +#define B3_SHAPE_CODE B3_MAKE_ID('S','H','A','P') +#define B3_ARRAY_CODE B3_MAKE_ID('A','R','A','Y') +#define B3_SBMATERIAL_CODE B3_MAKE_ID('S','B','M','T') +#define B3_SBNODE_CODE B3_MAKE_ID('S','B','N','D') +#define B3_DYNAMICSWORLD_CODE B3_MAKE_ID('D','W','L','D') +#define B3_DNA_CODE B3_MAKE_ID('D','N','A','1') + + +struct b3PointerUid +{ + union + { + void* m_ptr; + int m_uniqueIds[2]; + }; +}; + +///The b3DefaultSerializer is the main Bullet serialization class. +///The constructor takes an optional argument for backwards compatibility, it is recommended to leave this empty/zero. +class b3DefaultSerializer : public b3Serializer +{ + + + b3AlignedObjectArray mTypes; + b3AlignedObjectArray mStructs; + b3AlignedObjectArray mTlens; + b3HashMap mStructReverse; + b3HashMap mTypeLookup; + + + b3HashMap m_chunkP; + + b3HashMap m_nameMap; + + b3HashMap m_uniquePointers; + int m_uniqueIdGenerator; + + int m_totalSize; + unsigned char* m_buffer; + int m_currentSize; + void* m_dna; + int m_dnaLength; + + int m_serializationFlags; + + + b3AlignedObjectArray m_chunkPtrs; + +protected: + + virtual void* findPointer(void* oldPtr) + { + void** ptr = m_chunkP.find(oldPtr); + if (ptr && *ptr) + return *ptr; + return 0; + } + + + + + + void writeDNA() + { + b3Chunk* dnaChunk = allocate(m_dnaLength,1); + memcpy(dnaChunk->m_oldPtr,m_dna,m_dnaLength); + finalizeChunk(dnaChunk,"DNA1",B3_DNA_CODE, m_dna); + } + + int getReverseType(const char *type) const + { + + b3HashString key(type); + const int* valuePtr = mTypeLookup.find(key); + if (valuePtr) + return *valuePtr; + + return -1; + } + + void initDNA(const char* bdnaOrg,int dnalen) + { + ///was already initialized + if (m_dna) + return; + + int littleEndian= 1; + littleEndian= ((char*)&littleEndian)[0]; + + + m_dna = b3AlignedAlloc(dnalen,16); + memcpy(m_dna,bdnaOrg,dnalen); + m_dnaLength = dnalen; + + int *intPtr=0; + short *shtPtr=0; + char *cp = 0;int dataLen =0; + intPtr = (int*)m_dna; + + /* + SDNA (4 bytes) (magic number) + NAME (4 bytes) + (4 bytes) amount of names (int) + + + */ + + if (strncmp((const char*)m_dna, "SDNA", 4)==0) + { + // skip ++ NAME + intPtr++; intPtr++; + } + + // Parse names + if (!littleEndian) + *intPtr = b3SwapEndian(*intPtr); + + dataLen = *intPtr; + + intPtr++; + + cp = (char*)intPtr; + int i; + for ( i=0; i amount of types (int) + + + */ + + intPtr = (int*)cp; + b3Assert(strncmp(cp, "TYPE", 4)==0); intPtr++; + + if (!littleEndian) + *intPtr = b3SwapEndian(*intPtr); + + dataLen = *intPtr; + intPtr++; + + + cp = (char*)intPtr; + for (i=0; i (short) the lengths of types + + */ + + // Parse type lens + intPtr = (int*)cp; + b3Assert(strncmp(cp, "TLEN", 4)==0); intPtr++; + + dataLen = (int)mTypes.size(); + + shtPtr = (short*)intPtr; + for (i=0; i amount of structs (int) + + + + + + + */ + + intPtr = (int*)shtPtr; + cp = (char*)intPtr; + b3Assert(strncmp(cp, "STRC", 4)==0); intPtr++; + + if (!littleEndian) + *intPtr = b3SwapEndian(*intPtr); + dataLen = *intPtr ; + intPtr++; + + + shtPtr = (short*)intPtr; + for (i=0; im_length; + memcpy(currentPtr,m_chunkPtrs[i], curLength); + b3AlignedFree(m_chunkPtrs[i]); + currentPtr+=curLength; + mysize+=curLength; + } + } + + mTypes.clear(); + mStructs.clear(); + mTlens.clear(); + mStructReverse.clear(); + mTypeLookup.clear(); + m_chunkP.clear(); + m_nameMap.clear(); + m_uniquePointers.clear(); + m_chunkPtrs.clear(); + } + + virtual void* getUniquePointer(void*oldPtr) + { + if (!oldPtr) + return 0; + + b3PointerUid* uptr = (b3PointerUid*)m_uniquePointers.find(oldPtr); + if (uptr) + { + return uptr->m_ptr; + } + m_uniqueIdGenerator++; + + b3PointerUid uid; + uid.m_uniqueIds[0] = m_uniqueIdGenerator; + uid.m_uniqueIds[1] = m_uniqueIdGenerator; + m_uniquePointers.insert(oldPtr,uid); + return uid.m_ptr; + + } + + virtual const unsigned char* getBufferPointer() const + { + return m_buffer; + } + + virtual int getCurrentBufferSize() const + { + return m_currentSize; + } + + virtual void finalizeChunk(b3Chunk* chunk, const char* structType, int chunkCode,void* oldPtr) + { + if (!(m_serializationFlags&B3_SERIALIZE_NO_DUPLICATE_ASSERT)) + { + b3Assert(!findPointer(oldPtr)); + } + + chunk->m_dna_nr = getReverseType(structType); + + chunk->m_chunkCode = chunkCode; + + void* uniquePtr = getUniquePointer(oldPtr); + + m_chunkP.insert(oldPtr,uniquePtr);//chunk->m_oldPtr); + chunk->m_oldPtr = uniquePtr;//oldPtr; + + } + + + virtual unsigned char* internalAlloc(size_t size) + { + unsigned char* ptr = 0; + + if (m_totalSize) + { + ptr = m_buffer+m_currentSize; + m_currentSize += int(size); + b3Assert(m_currentSizem_chunkCode = 0; + chunk->m_oldPtr = data; + chunk->m_length = int(size)*numElements; + chunk->m_number = numElements; + + m_chunkPtrs.push_back(chunk); + + + return chunk; + } + + virtual const char* findNameForPointer(const void* ptr) const + { + const char*const * namePtr = m_nameMap.find(ptr); + if (namePtr && *namePtr) + return *namePtr; + return 0; + + } + + virtual void registerNameForPointer(const void* ptr, const char* name) + { + m_nameMap.insert(ptr,name); + } + + virtual void serializeName(const char* name) + { + if (name) + { + //don't serialize name twice + if (findPointer((void*)name)) + return; + + int len = b3StrLen(name); + if (len) + { + + int newLen = len+1; + int padding = ((newLen+3)&~3)-newLen; + newLen += padding; + + //serialize name string now + b3Chunk* chunk = allocate(sizeof(char),newLen); + char* destinationName = (char*)chunk->m_oldPtr; + for (int i=0;i +{ +public: + + btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); + +}; + +/// The bt32BitAxisSweep3 allows higher precision quantization and more objects compared to the btAxisSweep3 sweep and prune. +/// This comes at the cost of more memory per handle, and a bit slower performance. +/// It uses arrays rather then lists for storage of the 3 axis. +class bt32BitAxisSweep3 : public btAxisSweep3Internal +{ +public: + + bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); + +}; + +#endif diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/extern/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h similarity index 93% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h index cd6e1a8929e0..2c4d41bc041b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h @@ -16,8 +16,8 @@ // // 3. This notice may not be removed or altered from any source distribution. -#ifndef BT_AXIS_SWEEP_3_H -#define BT_AXIS_SWEEP_3_H +#ifndef BT_AXIS_SWEEP_3_INTERNAL_H +#define BT_AXIS_SWEEP_3_INTERNAL_H #include "LinearMath/btVector3.h" #include "btOverlappingPairCache.h" @@ -134,7 +134,7 @@ class btAxisSweep3Internal : public btBroadphaseInterface virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); + BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher); void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher); void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;} @@ -144,7 +144,7 @@ class btAxisSweep3Internal : public btBroadphaseInterface void processAllOverlappingPairs(btOverlapCallback* callback); //Broadphase Interface - virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); + virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr , int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher); virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; @@ -228,16 +228,16 @@ void btAxisSweep3::debugPrintAxis(int axis, bool checkCardinalit #endif //DEBUG_BROADPHASE template -btBroadphaseProxy* btAxisSweep3Internal::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy) +btBroadphaseProxy* btAxisSweep3Internal::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher) { (void)shapeType; - BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,multiSapProxy); + BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher); Handle* handle = getHandle(handleId); if (m_raycastAccelerator) { - btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0); + btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher); handle->m_dbvtProxy = rayProxy; } return handle; @@ -502,7 +502,7 @@ void btAxisSweep3Internal::freeHandle(BP_FP_INT_TYPE handle) template -BP_FP_INT_TYPE btAxisSweep3Internal::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy) +BP_FP_INT_TYPE btAxisSweep3Internal::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher) { // quantize the bounds BP_FP_INT_TYPE min[3], max[3]; @@ -520,7 +520,6 @@ BP_FP_INT_TYPE btAxisSweep3Internal::addHandle(const btVector3& pHandle->m_clientObject = pOwner; pHandle->m_collisionFilterGroup = collisionFilterGroup; pHandle->m_collisionFilterMask = collisionFilterMask; - pHandle->m_multiSapParentProxy = multiSapProxy; // compute current limit of edge arrays BP_FP_INT_TYPE limit = static_cast(m_numHandles * 2); @@ -1020,32 +1019,4 @@ void btAxisSweep3Internal::sortMaxUp(int axis, BP_FP_INT_TYPE ed } - - -//////////////////////////////////////////////////////////////////// - - -/// The btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase. -/// It uses arrays rather then lists for storage of the 3 axis. Also it operates using 16 bit integer coordinates instead of floats. -/// For large worlds and many objects, use bt32BitAxisSweep3 or btDbvtBroadphase instead. bt32BitAxisSweep3 has higher precision and allows more then 16384 objects at the cost of more memory and bit of performance. -class btAxisSweep3 : public btAxisSweep3Internal -{ -public: - - btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); - -}; - -/// The bt32BitAxisSweep3 allows higher precision quantization and more objects compared to the btAxisSweep3 sweep and prune. -/// This comes at the cost of more memory per handle, and a bit slower performance. -/// It uses arrays rather then lists for storage of the 3 axis. -class bt32BitAxisSweep3 : public btAxisSweep3Internal -{ -public: - - bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); - -}; - #endif - diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/extern/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h similarity index 94% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index f1bf00594d3b..fb68e0024e02 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -41,6 +41,10 @@ struct btBroadphaseRayCallback : public btBroadphaseAabbCallback btScalar m_lambda_max; virtual ~btBroadphaseRayCallback() {} + +protected: + + btBroadphaseRayCallback() {} }; #include "LinearMath/btVector3.h" @@ -53,7 +57,7 @@ class btBroadphaseInterface public: virtual ~btBroadphaseInterface() {} - virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy) =0; + virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher) =0; virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0; virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)=0; virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const =0; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp b/extern/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp similarity index 83% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp index f4d7341f8ddf..0fd4ef46be5b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp @@ -15,3 +15,4 @@ subject to the following restrictions: #include "btBroadphaseProxy.h" +BT_NOT_EMPTY_FILE // fix warning LNK4221: This object file does not define any previously undefined public symbols, so it will not be used by any link operation that consumes this library diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/extern/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h similarity index 94% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index bb58b8289361..f6e1202a697d 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -66,6 +66,7 @@ CONCAVE_SHAPES_START_HERE, EMPTY_SHAPE_PROXYTYPE, STATIC_PLANE_PROXYTYPE, CUSTOM_CONCAVE_SHAPE_TYPE, + SDF_SHAPE_PROXYTYPE=CUSTOM_CONCAVE_SHAPE_TYPE, CONCAVE_SHAPES_END_HERE, COMPOUND_SHAPE_PROXYTYPE, @@ -101,10 +102,10 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); //Usually the client btCollisionObject or Rigidbody class void* m_clientObject; - short int m_collisionFilterGroup; - short int m_collisionFilterMask; - void* m_multiSapParentProxy; - int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. + int m_collisionFilterGroup; + int m_collisionFilterMask; + + int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. btVector3 m_aabbMin; btVector3 m_aabbMax; @@ -115,18 +116,17 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); } //used for memory pools - btBroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0) + btBroadphaseProxy() :m_clientObject(0) { } - btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0) + btBroadphaseProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask) :m_clientObject(userPtr), m_collisionFilterGroup(collisionFilterGroup), m_collisionFilterMask(collisionFilterMask), m_aabbMin(aabbMin), m_aabbMax(aabbMax) { - m_multiSapParentProxy = multiSapParentProxy; } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp b/extern/bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/extern/bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h similarity index 100% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/extern/bullet/src/BulletCollision/BroadphaseCollision/btDbvt.cpp similarity index 95% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index 2ca20cdd8b8b..d791d0741821 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -229,25 +229,60 @@ static void fetchleaves(btDbvt* pdbvt, } // -static void split( const tNodeArray& leaves, - tNodeArray& left, - tNodeArray& right, +static bool leftOfAxis( const btDbvtNode* node, + const btVector3& org, + const btVector3& axis) +{ + return btDot(axis, node->volume.Center() - org) <= 0; +} + + +// Partitions leaves such that leaves[0, n) are on the +// left of axis, and leaves[n, count) are on the right +// of axis. returns N. +static int split( btDbvtNode** leaves, + int count, const btVector3& org, const btVector3& axis) { - left.resize(0); - right.resize(0); - for(int i=0,ni=leaves.size();ivolume.Center()-org)<0) - left.push_back(leaves[i]); - else - right.push_back(leaves[i]); + while(begin!=end && leftOfAxis(leaves[begin],org,axis)) + { + ++begin; + } + + if(begin==end) + { + break; + } + + while(begin!=end && !leftOfAxis(leaves[end-1],org,axis)) + { + --end; + } + + if(begin==end) + { + break; + } + + // swap out of place nodes + --end; + btDbvtNode* temp=leaves[begin]; + leaves[begin]=leaves[end]; + leaves[end]=temp; + ++begin; } + + return begin; } // -static btDbvtVolume bounds( const tNodeArray& leaves) +static btDbvtVolume bounds( btDbvtNode** leaves, + int count) { #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); @@ -257,7 +292,7 @@ static btDbvtVolume bounds( const tNodeArray& leaves) #else btDbvtVolume volume=leaves[0]->volume; #endif - for(int i=1,ni=leaves.size();ivolume,volume); } @@ -266,15 +301,16 @@ static btDbvtVolume bounds( const tNodeArray& leaves) // static void bottomup( btDbvt* pdbvt, - tNodeArray& leaves) + btDbvtNode** leaves, + int count) { - while(leaves.size()>1) + while(count>1) { btScalar minsize=SIMD_INFINITY; int minidx[2]={-1,-1}; - for(int i=0;ivolume,leaves[j]->volume)); if(szparent = p; n[1]->parent = p; leaves[minidx[0]] = p; - leaves.swap(minidx[1],leaves.size()-1); - leaves.pop_back(); + leaves[minidx[1]] = leaves[count-1]; + --count; } } // static btDbvtNode* topdown(btDbvt* pdbvt, - tNodeArray& leaves, + btDbvtNode** leaves, + int count, int bu_treshold) { static const btVector3 axis[]={btVector3(1,0,0), btVector3(0,1,0), btVector3(0,0,1)}; - if(leaves.size()>1) + btAssert(bu_treshold>2); + if(count>1) { - if(leaves.size()>bu_treshold) + if(count>bu_treshold) { - const btDbvtVolume vol=bounds(leaves); + const btDbvtVolume vol=bounds(leaves,count); const btVector3 org=vol.Center(); - tNodeArray sets[2]; + int partition; int bestaxis=-1; - int bestmidp=leaves.size(); + int bestmidp=count; int splitcount[3][2]={{0,0},{0,0},{0,0}}; int i; - for( i=0;ivolume.Center()-org; for(int j=0;j<3;++j) @@ -338,29 +376,23 @@ static btDbvtNode* topdown(btDbvt* pdbvt, } if(bestaxis>=0) { - sets[0].reserve(splitcount[bestaxis][0]); - sets[1].reserve(splitcount[bestaxis][1]); - split(leaves,sets[0],sets[1],org,axis[bestaxis]); + partition=split(leaves,count,org,axis[bestaxis]); + btAssert(partition!=0 && partition!=count); } else { - sets[0].reserve(leaves.size()/2+1); - sets[1].reserve(leaves.size()/2); - for(int i=0,ni=leaves.size();ichilds[0]=topdown(pdbvt,sets[0],bu_treshold); - node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); + node->childs[0]=topdown(pdbvt,&leaves[0],partition,bu_treshold); + node->childs[1]=topdown(pdbvt,&leaves[partition],count-partition,bu_treshold); node->childs[0]->parent=node; node->childs[1]->parent=node; return(node); } else { - bottomup(pdbvt,leaves); + bottomup(pdbvt,leaves,count); return(leaves[0]); } } @@ -444,7 +476,7 @@ void btDbvt::optimizeBottomUp() tNodeArray leaves; leaves.reserve(m_leaves); fetchleaves(this,m_root,leaves); - bottomup(this,leaves); + bottomup(this,&leaves[0],leaves.size()); m_root=leaves[0]; } } @@ -457,7 +489,7 @@ void btDbvt::optimizeTopDown(int bu_treshold) tNodeArray leaves; leaves.reserve(m_leaves); fetchleaves(this,m_root,leaves); - m_root=topdown(this,leaves,bu_treshold); + m_root=topdown(this,&leaves[0],leaves.size(),bu_treshold); } } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h b/extern/bullet/src/BulletCollision/BroadphaseCollision/btDbvt.h similarity index 94% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btDbvt.h index db4e482f2926..b5a0014580df 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -122,6 +122,7 @@ subject to the following restrictions: #error "DBVT_INT0_IMPL undefined" #endif + // // Defaults volumes // @@ -188,6 +189,9 @@ struct btDbvtNode }; }; +typedef btAlignedObjectArray btNodeStack; + + ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). ///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes. ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. @@ -263,7 +267,6 @@ struct btDbvt btAlignedObjectArray m_stkStack; - mutable btAlignedObjectArray m_rayTestStack; // Methods @@ -325,6 +328,16 @@ struct btDbvt void collideTV( const btDbvtNode* root, const btDbvtVolume& volume, DBVT_IPOLICY) const; + + DBVT_PREFIX + void collideTVNoStackAlloc( const btDbvtNode* root, + const btDbvtVolume& volume, + btNodeStack& stack, + DBVT_IPOLICY) const; + + + + ///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc) ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time DBVT_PREFIX @@ -343,6 +356,7 @@ struct btDbvt btScalar lambda_max, const btVector3& aabbMin, const btVector3& aabbMax, + btAlignedObjectArray& stack, DBVT_IPOLICY) const; DBVT_PREFIX @@ -917,39 +931,78 @@ inline void btDbvt::collideTT( const btDbvtNode* root0, } #endif -// DBVT_PREFIX inline void btDbvt::collideTV( const btDbvtNode* root, const btDbvtVolume& vol, DBVT_IPOLICY) const { DBVT_CHECKTYPE - if(root) - { - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); - btAlignedObjectArray stack; - stack.resize(0); - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; - stack.pop_back(); - if(Intersect(n->volume,volume)) + if(root) + { + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); + btAlignedObjectArray stack; + stack.resize(0); +#ifndef BT_DISABLE_STACK_TEMP_MEMORY + char tempmemory[SIMPLE_STACKSIZE*sizeof(const btDbvtNode*)]; + stack.initializeFromBuffer(tempmemory, 0, SIMPLE_STACKSIZE); +#else + stack.reserve(SIMPLE_STACKSIZE); +#endif //BT_DISABLE_STACK_TEMP_MEMORY + + stack.push_back(root); + do { + const btDbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(Intersect(n->volume,volume)) + { + if(n->isinternal()) { - if(n->isinternal()) - { - stack.push_back(n->childs[0]); - stack.push_back(n->childs[1]); - } - else - { - policy.Process(n); - } + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); } - } while(stack.size()>0); - } + else + { + policy.Process(n); + } + } + } while(stack.size()>0); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideTVNoStackAlloc( const btDbvtNode* root, + const btDbvtVolume& vol, + btNodeStack& stack, + DBVT_IPOLICY) const +{ + DBVT_CHECKTYPE + if(root) + { + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); + stack.resize(0); + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(Intersect(n->volume,volume)) + { + if(n->isinternal()) + { + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); + } + else + { + policy.Process(n); + } + } + } while(stack.size()>0); + } } + DBVT_PREFIX inline void btDbvt::rayTestInternal( const btDbvtNode* root, const btVector3& rayFrom, @@ -959,7 +1012,8 @@ inline void btDbvt::rayTestInternal( const btDbvtNode* root, btScalar lambda_max, const btVector3& aabbMin, const btVector3& aabbMax, - DBVT_IPOLICY) const + btAlignedObjectArray& stack, + DBVT_IPOLICY ) const { (void) rayTo; DBVT_CHECKTYPE @@ -969,7 +1023,6 @@ inline void btDbvt::rayTestInternal( const btDbvtNode* root, int depth=1; int treshold=DOUBLE_STACKSIZE-2; - btAlignedObjectArray& stack = m_rayTestStack; stack.resize(DOUBLE_STACKSIZE); stack[0]=root; btVector3 bounds[2]; @@ -1031,7 +1084,12 @@ inline void btDbvt::rayTest( const btDbvtNode* root, int depth=1; int treshold=DOUBLE_STACKSIZE-2; + char tempmemory[DOUBLE_STACKSIZE * sizeof(const btDbvtNode*)]; +#ifndef BT_DISABLE_STACK_TEMP_MEMORY + stack.initializeFromBuffer(tempmemory, DOUBLE_STACKSIZE, DOUBLE_STACKSIZE); +#else//BT_DISABLE_STACK_TEMP_MEMORY stack.resize(DOUBLE_STACKSIZE); +#endif //BT_DISABLE_STACK_TEMP_MEMORY stack[0]=root; btVector3 bounds[2]; do { diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/extern/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp similarity index 95% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp index 75cfac64368d..4d12b1c9c79b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -16,6 +16,7 @@ subject to the following restrictions: ///btDbvtBroadphase implementation by Nathanael Presson #include "btDbvtBroadphase.h" +#include "LinearMath/btThreads.h" // // Profiling @@ -142,6 +143,11 @@ btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) { m_stageRoots[i]=0; } +#if BT_THREADSAFE + m_rayTestStacks.resize(BT_MAX_THREAD_COUNT); +#else + m_rayTestStacks.resize(1); +#endif #if DBVT_BP_PROFILE clear(m_profiling); #endif @@ -162,10 +168,9 @@ btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax, int /*shapeType*/, void* userPtr, - short int collisionFilterGroup, - short int collisionFilterMask, - btDispatcher* /*dispatcher*/, - void* /*multiSapProxy*/) + int collisionFilterGroup, + int collisionFilterMask, + btDispatcher* /*dispatcher*/) { btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( aabbMin,aabbMax,userPtr, collisionFilterGroup, @@ -227,6 +232,23 @@ struct BroadphaseRayTester : btDbvt::ICollide void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) { BroadphaseRayTester callback(rayCallback); + btAlignedObjectArray* stack = &m_rayTestStacks[0]; +#if BT_THREADSAFE + // for this function to be threadsafe, each thread must have a separate copy + // of this stack. This could be thread-local static to avoid dynamic allocations, + // instead of just a local. + int threadIndex = btGetCurrentThreadIndex(); + btAlignedObjectArray localStack; + if (threadIndex < m_rayTestStacks.size()) + { + // use per-thread preallocated stack if possible to avoid dynamic allocations + stack = &m_rayTestStacks[threadIndex]; + } + else + { + stack = &localStack; + } +#endif m_sets[0].rayTestInternal( m_sets[0].m_root, rayFrom, @@ -236,6 +258,7 @@ void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, rayCallback.m_lambda_max, aabbMin, aabbMax, + *stack, callback); m_sets[1].rayTestInternal( m_sets[1].m_root, @@ -246,6 +269,7 @@ void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, rayCallback.m_lambda_max, aabbMin, aabbMax, + *stack, callback); } @@ -520,7 +544,9 @@ void btDbvtBroadphase::collide(btDispatcher* dispatcher) btDbvtProxy* current=m_stageRoots[m_stageCurrent]; if(current) { +#if DBVT_BP_ACCURATESLEEPING btDbvtTreeCollider collider(this); +#endif do { btDbvtProxy* next=current->links[1]; listremove(current,m_stageRoots[current->stage]); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/extern/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h similarity index 95% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h index 18b64ad0e57f..8feb95d51fec 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -47,7 +47,7 @@ struct btDbvtProxy : btBroadphaseProxy btDbvtProxy* links[2]; int stage; /* ctor */ - btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : + btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask) : btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask) { links[0]=links[1]=0; @@ -87,6 +87,7 @@ struct btDbvtBroadphase : btBroadphaseInterface bool m_releasepaircache; // Release pair cache on delete bool m_deferedcollide; // Defere dynamic/static collision to collide call bool m_needcleanup; // Need to run cleanup? + btAlignedObjectArray< btAlignedObjectArray > m_rayTestStacks; #if DBVT_BP_PROFILE btClock m_clock; struct { @@ -104,7 +105,7 @@ struct btDbvtBroadphase : btBroadphaseInterface void optimize(); /* btBroadphaseInterface Implementation */ - btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); + btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask,btDispatcher* dispatcher); virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp b/extern/bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.h similarity index 91% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 89c307d14ca5..a0e4c189270a 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -46,7 +46,8 @@ struct btDispatcherInfo m_useEpa(true), m_allowedCcdPenetration(btScalar(0.04)), m_useConvexConservativeDistanceUtil(false), - m_convexConservativeDistanceThreshold(0.0f) + m_convexConservativeDistanceThreshold(0.0f), + m_deterministicOverlappingPairs(false) { } @@ -62,6 +63,13 @@ struct btDispatcherInfo btScalar m_allowedCcdPenetration; bool m_useConvexConservativeDistanceUtil; btScalar m_convexConservativeDistanceThreshold; + bool m_deterministicOverlappingPairs; +}; + +enum ebtDispatcherQueryType +{ + BT_CONTACT_POINT_ALGORITHMS = 1, + BT_CLOSEST_POINT_ALGORITHMS = 2 }; ///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs. @@ -73,7 +81,7 @@ class btDispatcher public: virtual ~btDispatcher() ; - virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold=0) = 0; + virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType) = 0; virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1)=0; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/extern/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp similarity index 88% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index ad69fcbd712b..da13357029fe 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -372,10 +372,10 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro return userData; } //#include - +#include "LinearMath/btQuickprof.h" void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) { - + BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs"); int i; // printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size()); @@ -395,6 +395,70 @@ void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* } } +struct MyPairIndex +{ + int m_orgIndex; + int m_uidA0; + int m_uidA1; +}; + +class MyPairIndeSortPredicate +{ +public: + + bool operator() ( const MyPairIndex& a, const MyPairIndex& b ) const + { + const int uidA0 = a.m_uidA0; + const int uidB0 = b.m_uidA0; + const int uidA1 = a.m_uidA1; + const int uidB1 = b.m_uidA1; + return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1); + } +}; + +void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo) +{ + if (dispatchInfo.m_deterministicOverlappingPairs) + { + btBroadphasePairArray& pa = getOverlappingPairArray(); + btAlignedObjectArray indices; + { + BT_PROFILE("sortOverlappingPairs"); + indices.resize(pa.size()); + for (int i=0;im_uniqueId : -1; + const int uidA1 = p.m_pProxy1 ? p.m_pProxy1->m_uniqueId : -1; + + indices[i].m_uidA0 = uidA0; + indices[i].m_uidA1 = uidA1; + indices[i].m_orgIndex = i; + } + indices.quickSort(MyPairIndeSortPredicate()); + } + { + BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs"); + int i; + for (i=0;iprocessOverlap(*pair)) + { + removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher); + } else + { + i++; + } + } + } + } else + { + processAllOverlappingPairs(callback, dispatcher); + } +} + + void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) { ///need to keep hashmap in sync with pair address, so rebuild all diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h similarity index 95% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index 146142704761..2551ac0e5ca4 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -78,6 +78,10 @@ class btOverlappingPairCache : public btOverlappingPairCallback virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0; + virtual void processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo) + { + processAllOverlappingPairs(callback, dispatcher); + } virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0; virtual bool hasDeferredRemoval() = 0; @@ -90,7 +94,8 @@ class btOverlappingPairCache : public btOverlappingPairCallback }; /// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com -class btHashedOverlappingPairCache : public btOverlappingPairCache + +ATTRIBUTE_ALIGNED16(class) btHashedOverlappingPairCache : public btOverlappingPairCache { btBroadphasePairArray m_overlappingPairArray; btOverlapFilterCallback* m_overlapFilterCallback; @@ -103,6 +108,8 @@ class btHashedOverlappingPairCache : public btOverlappingPairCache public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + btHashedOverlappingPairCache(); virtual ~btHashedOverlappingPairCache(); @@ -141,6 +148,8 @@ class btHashedOverlappingPairCache : public btOverlappingPairCache virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); + virtual void processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo); + virtual btBroadphasePair* getOverlappingPairArrayPtr() { return &m_overlappingPairArray[0]; @@ -212,10 +221,9 @@ class btHashedOverlappingPairCache : public btOverlappingPairCache */ - - SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2) + SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2) { - int key = static_cast(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16)); + unsigned int key = proxyId1 | (proxyId2 << 16); // Thomas Wang's hash key += ~(key << 15); @@ -224,13 +232,11 @@ class btHashedOverlappingPairCache : public btOverlappingPairCache key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); - return static_cast(key); + return key; } - - SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash) { int proxyId1 = proxy0->getUid(); diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h b/extern/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h similarity index 97% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h index 9c7b6f81367d..3e069fa5e283 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h @@ -23,6 +23,9 @@ struct btBroadphasePair; ///The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache. class btOverlappingPairCallback { +protected: + btOverlappingPairCallback() {} + public: virtual ~btOverlappingPairCallback() { diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/extern/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp similarity index 99% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp index 889216df5092..875d89c53ec9 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -107,6 +107,8 @@ void btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btV v = unQuantize(vecIn); m_bvhAabbMin.setMin(v-clampValue); } + aabbSize = m_bvhAabbMax - m_bvhAabbMin; + m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize; { quantize(vecIn,m_bvhAabbMax,true); v = unQuantize(vecIn); @@ -1334,6 +1336,8 @@ const char* btQuantizedBvh::serialize(void* dataBuffer, btSerializer* serializer memPtr->m_escapeIndex = m_contiguousNodes[i].m_escapeIndex; memPtr->m_subPart = m_contiguousNodes[i].m_subPart; memPtr->m_triangleIndex = m_contiguousNodes[i].m_triangleIndex; + // Fill padding with zeros to appease msan. + memset(memPtr->m_pad, 0, sizeof(memPtr->m_pad)); } serializer->finalizeChunk(chunk,"btOptimizedBvhNodeData",BT_ARRAY_CODE,(void*)&m_contiguousNodes[0]); } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h b/extern/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h similarity index 99% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h index 78382da79f0a..3dd5ac9bb65c 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @@ -169,7 +169,7 @@ typedef btAlignedObjectArray BvhSubtreeInfoArray; ///The btQuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU. -///It is used by the btBvhTriangleMeshShape as midphase, and by the btMultiSapBroadphase. +///It is used by the btBvhTriangleMeshShape as midphase. ///It is recommended to use quantization for better performance and lower memory requirements. ATTRIBUTE_ALIGNED16(class) btQuantizedBvh { diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/extern/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp similarity index 98% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index 752fcd0fef27..f1d5f5476ecf 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -84,7 +84,7 @@ btSimpleBroadphase::~btSimpleBroadphase() } -btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* /*dispatcher*/,void* multiSapProxy) +btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr , int collisionFilterGroup, int collisionFilterMask, btDispatcher* /*dispatcher*/) { if (m_numHandles >= m_maxHandles) { @@ -94,7 +94,7 @@ btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& aabbMin, btAssert(aabbMin[0]<= aabbMax[0] && aabbMin[1]<= aabbMax[1] && aabbMin[2]<= aabbMax[2]); int newHandleIndex = allocHandle(); - btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy); + btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask); return proxy; } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/extern/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h similarity index 94% rename from extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h rename to extern/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index 7cb3c40a0438..d7a18e400a45 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/extern/bullet/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -29,8 +29,8 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy btSimpleBroadphaseProxy() {}; - btSimpleBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,void* multiSapProxy) - :btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy) + btSimpleBroadphaseProxy(const btVector3& minpt,const btVector3& maxpt,int shapeType,void* userPtr, int collisionFilterGroup, int collisionFilterMask) + :btBroadphaseProxy(minpt,maxpt,userPtr,collisionFilterGroup,collisionFilterMask) { (void)shapeType; } @@ -127,7 +127,7 @@ class btSimpleBroadphase : public btBroadphaseInterface static bool aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1); - virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy); + virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr , int collisionFilterGroup, int collisionFilterMask, btDispatcher* dispatcher); virtual void calculateOverlappingPairs(btDispatcher* dispatcher); diff --git a/extern/bullet/src/BulletCollision/CMakeLists.txt b/extern/bullet/src/BulletCollision/CMakeLists.txt new file mode 100644 index 000000000000..f5d725562cea --- /dev/null +++ b/extern/bullet/src/BulletCollision/CMakeLists.txt @@ -0,0 +1,294 @@ +INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src ) + +SET(BulletCollision_SRCS + BroadphaseCollision/btAxisSweep3.cpp + BroadphaseCollision/btBroadphaseProxy.cpp + BroadphaseCollision/btCollisionAlgorithm.cpp + BroadphaseCollision/btDbvt.cpp + BroadphaseCollision/btDbvtBroadphase.cpp + BroadphaseCollision/btDispatcher.cpp + BroadphaseCollision/btOverlappingPairCache.cpp + BroadphaseCollision/btQuantizedBvh.cpp + BroadphaseCollision/btSimpleBroadphase.cpp + CollisionDispatch/btActivatingCollisionAlgorithm.cpp + CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp + CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp + CollisionDispatch/btBoxBoxDetector.cpp + CollisionDispatch/btCollisionDispatcher.cpp + CollisionDispatch/btCollisionDispatcherMt.cpp + CollisionDispatch/btCollisionObject.cpp + CollisionDispatch/btCollisionWorld.cpp + CollisionDispatch/btCollisionWorldImporter.cpp + CollisionDispatch/btCompoundCollisionAlgorithm.cpp + CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp + CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp + CollisionDispatch/btConvexConvexAlgorithm.cpp + CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp + CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp + CollisionDispatch/btDefaultCollisionConfiguration.cpp + CollisionDispatch/btEmptyCollisionAlgorithm.cpp + CollisionDispatch/btGhostObject.cpp + CollisionDispatch/btHashedSimplePairCache.cpp + CollisionDispatch/btInternalEdgeUtility.cpp + CollisionDispatch/btInternalEdgeUtility.h + CollisionDispatch/btManifoldResult.cpp + CollisionDispatch/btSimulationIslandManager.cpp + CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp + CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp + CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp + CollisionDispatch/btUnionFind.cpp + CollisionDispatch/SphereTriangleDetector.cpp + CollisionShapes/btBoxShape.cpp + CollisionShapes/btBox2dShape.cpp + CollisionShapes/btBvhTriangleMeshShape.cpp + CollisionShapes/btCapsuleShape.cpp + CollisionShapes/btCollisionShape.cpp + CollisionShapes/btCompoundShape.cpp + CollisionShapes/btConcaveShape.cpp + CollisionShapes/btConeShape.cpp + CollisionShapes/btConvexHullShape.cpp + CollisionShapes/btConvexInternalShape.cpp + CollisionShapes/btConvexPointCloudShape.cpp + CollisionShapes/btConvexPolyhedron.cpp + CollisionShapes/btConvexShape.cpp + CollisionShapes/btConvex2dShape.cpp + CollisionShapes/btConvexTriangleMeshShape.cpp + CollisionShapes/btCylinderShape.cpp + CollisionShapes/btEmptyShape.cpp + CollisionShapes/btHeightfieldTerrainShape.cpp + CollisionShapes/btMiniSDF.cpp + CollisionShapes/btMinkowskiSumShape.cpp + CollisionShapes/btMultimaterialTriangleMeshShape.cpp + CollisionShapes/btMultiSphereShape.cpp + CollisionShapes/btOptimizedBvh.cpp + CollisionShapes/btPolyhedralConvexShape.cpp + CollisionShapes/btScaledBvhTriangleMeshShape.cpp + CollisionShapes/btSdfCollisionShape.cpp + CollisionShapes/btShapeHull.cpp + CollisionShapes/btSphereShape.cpp + CollisionShapes/btStaticPlaneShape.cpp + CollisionShapes/btStridingMeshInterface.cpp + CollisionShapes/btTetrahedronShape.cpp + CollisionShapes/btTriangleBuffer.cpp + CollisionShapes/btTriangleCallback.cpp + CollisionShapes/btTriangleIndexVertexArray.cpp + CollisionShapes/btTriangleIndexVertexMaterialArray.cpp + CollisionShapes/btTriangleMesh.cpp + CollisionShapes/btTriangleMeshShape.cpp + CollisionShapes/btUniformScalingShape.cpp + Gimpact/btContactProcessing.cpp + Gimpact/btGenericPoolAllocator.cpp + Gimpact/btGImpactBvh.cpp + Gimpact/btGImpactCollisionAlgorithm.cpp + Gimpact/btGImpactQuantizedBvh.cpp + Gimpact/btGImpactShape.cpp + Gimpact/btTriangleShapeEx.cpp + Gimpact/gim_box_set.cpp + Gimpact/gim_contact.cpp + Gimpact/gim_memory.cpp + Gimpact/gim_tri_collision.cpp + NarrowPhaseCollision/btContinuousConvexCollision.cpp + NarrowPhaseCollision/btConvexCast.cpp + NarrowPhaseCollision/btGjkConvexCast.cpp + NarrowPhaseCollision/btGjkEpa2.cpp + NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp + NarrowPhaseCollision/btGjkPairDetector.cpp + NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp + NarrowPhaseCollision/btPersistentManifold.cpp + NarrowPhaseCollision/btRaycastCallback.cpp + NarrowPhaseCollision/btSubSimplexConvexCast.cpp + NarrowPhaseCollision/btVoronoiSimplexSolver.cpp + NarrowPhaseCollision/btPolyhedralContactClipping.cpp +) + +SET(Root_HDRS + ../btBulletCollisionCommon.h +) +SET(BroadphaseCollision_HDRS + BroadphaseCollision/btAxisSweep3Internal.h + BroadphaseCollision/btAxisSweep3.h + BroadphaseCollision/btBroadphaseInterface.h + BroadphaseCollision/btBroadphaseProxy.h + BroadphaseCollision/btCollisionAlgorithm.h + BroadphaseCollision/btDbvt.h + BroadphaseCollision/btDbvtBroadphase.h + BroadphaseCollision/btDispatcher.h + BroadphaseCollision/btOverlappingPairCache.h + BroadphaseCollision/btOverlappingPairCallback.h + BroadphaseCollision/btQuantizedBvh.h + BroadphaseCollision/btSimpleBroadphase.h +) +SET(CollisionDispatch_HDRS + CollisionDispatch/btActivatingCollisionAlgorithm.h + CollisionDispatch/btBoxBoxCollisionAlgorithm.h + CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h + CollisionDispatch/btBoxBoxDetector.h + CollisionDispatch/btCollisionConfiguration.h + CollisionDispatch/btCollisionCreateFunc.h + CollisionDispatch/btCollisionDispatcher.h + CollisionDispatch/btCollisionDispatcherMt.h + CollisionDispatch/btCollisionObject.h + CollisionDispatch/btCollisionObjectWrapper.h + CollisionDispatch/btCollisionWorld.h + CollisionDispatch/btCollisionWorldImporter.h + CollisionDispatch/btCompoundCollisionAlgorithm.h + CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h + CollisionDispatch/btConvexConcaveCollisionAlgorithm.h + CollisionDispatch/btConvexConvexAlgorithm.h + CollisionDispatch/btConvex2dConvex2dAlgorithm.h + CollisionDispatch/btConvexPlaneCollisionAlgorithm.h + CollisionDispatch/btDefaultCollisionConfiguration.h + CollisionDispatch/btEmptyCollisionAlgorithm.h + CollisionDispatch/btGhostObject.h + CollisionDispatch/btHashedSimplePairCache.h + CollisionDispatch/btManifoldResult.h + CollisionDispatch/btSimulationIslandManager.h + CollisionDispatch/btSphereBoxCollisionAlgorithm.h + CollisionDispatch/btSphereSphereCollisionAlgorithm.h + CollisionDispatch/btSphereTriangleCollisionAlgorithm.h + CollisionDispatch/btUnionFind.h + CollisionDispatch/SphereTriangleDetector.h +) +SET(CollisionShapes_HDRS + CollisionShapes/btBoxShape.h + CollisionShapes/btBox2dShape.h + CollisionShapes/btBvhTriangleMeshShape.h + CollisionShapes/btCapsuleShape.h + CollisionShapes/btCollisionMargin.h + CollisionShapes/btCollisionShape.h + CollisionShapes/btCompoundShape.h + CollisionShapes/btConcaveShape.h + CollisionShapes/btConeShape.h + CollisionShapes/btConvexHullShape.h + CollisionShapes/btConvexInternalShape.h + CollisionShapes/btConvexPointCloudShape.h + CollisionShapes/btConvexPolyhedron.h + CollisionShapes/btConvexShape.h + CollisionShapes/btConvex2dShape.h + CollisionShapes/btConvexTriangleMeshShape.h + CollisionShapes/btCylinderShape.h + CollisionShapes/btEmptyShape.h + CollisionShapes/btHeightfieldTerrainShape.h + CollisionShapes/btMaterial.h + CollisionShapes/btMinkowskiSumShape.h + CollisionShapes/btMultimaterialTriangleMeshShape.h + CollisionShapes/btMultiSphereShape.h + CollisionShapes/btOptimizedBvh.h + CollisionShapes/btPolyhedralConvexShape.h + CollisionShapes/btScaledBvhTriangleMeshShape.h + CollisionShapes/btShapeHull.h + CollisionShapes/btSphereShape.h + CollisionShapes/btStaticPlaneShape.h + CollisionShapes/btStridingMeshInterface.h + CollisionShapes/btTetrahedronShape.h + CollisionShapes/btTriangleBuffer.h + CollisionShapes/btTriangleCallback.h + CollisionShapes/btTriangleIndexVertexArray.h + CollisionShapes/btTriangleIndexVertexMaterialArray.h + CollisionShapes/btTriangleInfoMap.h + CollisionShapes/btTriangleMesh.h + CollisionShapes/btTriangleMeshShape.h + CollisionShapes/btTriangleShape.h + CollisionShapes/btUniformScalingShape.h +) +SET(Gimpact_HDRS + Gimpact/btBoxCollision.h + Gimpact/btClipPolygon.h + Gimpact/btContactProcessingStructs.h + Gimpact/btContactProcessing.h + Gimpact/btGenericPoolAllocator.h + Gimpact/btGeometryOperations.h + Gimpact/btGImpactBvhStructs.h + Gimpact/btGImpactBvh.h + Gimpact/btGImpactCollisionAlgorithm.h + Gimpact/btGImpactMassUtil.h + Gimpact/btGImpactQuantizedBvhStructs.h + Gimpact/btGImpactQuantizedBvh.h + Gimpact/btGImpactShape.h + Gimpact/btQuantization.h + Gimpact/btTriangleShapeEx.h + Gimpact/gim_array.h + Gimpact/gim_basic_geometry_operations.h + Gimpact/gim_bitset.h + Gimpact/gim_box_collision.h + Gimpact/gim_box_set.h + Gimpact/gim_clip_polygon.h + Gimpact/gim_contact.h + Gimpact/gim_geom_types.h + Gimpact/gim_geometry.h + Gimpact/gim_hash_table.h + Gimpact/gim_linear_math.h + Gimpact/gim_math.h + Gimpact/gim_memory.h + Gimpact/gim_radixsort.h + Gimpact/gim_tri_collision.h +) +SET(NarrowPhaseCollision_HDRS + NarrowPhaseCollision/btContinuousConvexCollision.h + NarrowPhaseCollision/btConvexCast.h + NarrowPhaseCollision/btConvexPenetrationDepthSolver.h + NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h + NarrowPhaseCollision/btGjkConvexCast.h + NarrowPhaseCollision/btGjkEpa2.h + NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h + NarrowPhaseCollision/btGjkPairDetector.h + NarrowPhaseCollision/btManifoldPoint.h + NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h + NarrowPhaseCollision/btPersistentManifold.h + NarrowPhaseCollision/btPointCollector.h + NarrowPhaseCollision/btRaycastCallback.h + NarrowPhaseCollision/btSimplexSolverInterface.h + NarrowPhaseCollision/btSubSimplexConvexCast.h + NarrowPhaseCollision/btVoronoiSimplexSolver.h + NarrowPhaseCollision/btPolyhedralContactClipping.h +) + +SET(BulletCollision_HDRS + ${Root_HDRS} + ${BroadphaseCollision_HDRS} + ${CollisionDispatch_HDRS} + ${CollisionShapes_HDRS} + ${Gimpact_HDRS} + ${NarrowPhaseCollision_HDRS} +) + + +ADD_LIBRARY(BulletCollision ${BulletCollision_SRCS} ${BulletCollision_HDRS}) +SET_TARGET_PROPERTIES(BulletCollision PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(BulletCollision PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + TARGET_LINK_LIBRARIES(BulletCollision LinearMath) +ENDIF (BUILD_SHARED_LIBS) + + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + #INSTALL of other files requires CMake 2.6 + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletCollision DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletCollision RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + INSTALL(FILES ../btBulletCollisionCommon.h +DESTINATION ${INCLUDE_INSTALL_DIR}/BulletCollision) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(BulletCollision PROPERTIES FRAMEWORK true) + + SET_TARGET_PROPERTIES(BulletCollision PROPERTIES PUBLIC_HEADER "${Root_HDRS}") + # Have to list out sub-directories manually: + SET_PROPERTY(SOURCE ${BroadphaseCollision_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/BroadphaseCollision) + SET_PROPERTY(SOURCE ${CollisionDispatch_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/CollisionDispatch) + SET_PROPERTY(SOURCE ${CollisionShapes_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/CollisionShapes) + SET_PROPERTY(SOURCE ${Gimpact_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Gimpact) + SET_PROPERTY(SOURCE ${NarrowPhaseCollision_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/NarrowPhaseCollision) + + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp similarity index 78% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index 63401780970f..e5bac8438edb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -100,45 +100,56 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po btScalar radiusWithThreshold = radius + contactBreakingThreshold; btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); - normal.normalize(); - btVector3 p1ToCentre = sphereCenter - vertices[0]; - btScalar distanceFromPlane = p1ToCentre.dot(normal); - if (distanceFromPlane < btScalar(0.)) + btScalar l2 = normal.length2(); + bool hasContact = false; + btVector3 contactPoint; + + if (l2 >= SIMD_EPSILON*SIMD_EPSILON) { - //triangle facing the other way - distanceFromPlane *= btScalar(-1.); - normal *= btScalar(-1.); - } + normal /= btSqrt(l2); + + btVector3 p1ToCentre = sphereCenter - vertices[0]; + btScalar distanceFromPlane = p1ToCentre.dot(normal); + + if (distanceFromPlane < btScalar(0.)) + { + //triangle facing the other way + distanceFromPlane *= btScalar(-1.); + normal *= btScalar(-1.); + } - bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; + bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; - // Check for contact / intersection - bool hasContact = false; - btVector3 contactPoint; - if (isInsideContactPlane) { - if (facecontains(sphereCenter,vertices,normal)) { - // Inside the contact wedge - touches a point on the shell plane - hasContact = true; - contactPoint = sphereCenter - normal*distanceFromPlane; - } else { - // Could be inside one of the contact capsules - btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold; - 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,sphereCenter, nearestOnEdge); - if (distanceSqr < contactCapsuleRadiusSqr) { - // Yep, we're inside a capsule - hasContact = true; - contactPoint = nearestOnEdge; + // Check for contact / intersection + + if (isInsideContactPlane) { + if (facecontains(sphereCenter, vertices, normal)) { + // Inside the contact wedge - touches a point on the shell plane + hasContact = true; + contactPoint = sphereCenter - normal*distanceFromPlane; + } + else { + // Could be inside one of the contact capsules + btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold; + btScalar minDistSqr = contactCapsuleRadiusSqr; + 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, sphereCenter, nearestOnEdge); + if (distanceSqr < minDistSqr) { + // Yep, we're inside a capsule, and record the capsule with smallest distance + minDistSqr = distanceSqr; + hasContact = true; + contactPoint = nearestOnEdge; + } + } - } } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h similarity index 99% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h index 489812b96631..0e19f1ea356c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h @@ -24,12 +24,13 @@ class btActivatingCollisionAlgorithm : public btCollisionAlgorithm // btCollisionObject* m_colObj0; // btCollisionObject* m_colObj1; -public: +protected: btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci); btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap); +public: virtual ~btActivatingCollisionAlgorithm(); }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp similarity index 99% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp index c1da9f36cfb8..cabbb0bf6a04 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp @@ -174,7 +174,7 @@ static btScalar FindMaxSeparation(int* edgeIndex, // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; - btScalar maxDot; + btScalar maxDot; if( count1 > 0 ) edge = (int) dLocal1.maxDot( normals1, count1, maxDot); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h similarity index 93% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h index 66949849448e..35f77d4e6543 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -40,6 +40,9 @@ class btCollisionConfiguration virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0; + virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0; + + }; #endif //BT_COLLISION_CONFIGURATION diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp similarity index 83% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 3b6913c0e1ec..9889f0de7054 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: #include "btCollisionDispatcher.h" - +#include "LinearMath/btQuickprof.h" #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" @@ -50,8 +50,10 @@ m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESH { for (int j=0;jgetCollisionAlgorithmCreateFunc(i,j); - btAssert(m_doubleDispatch[i][j]); + m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j); + btAssert(m_doubleDispatchContactPoints[i][j]); + m_doubleDispatchClosestPoints[i][j] = m_collisionConfiguration->getClosestPointsAlgorithmCreateFunc(i, j); + } } @@ -61,7 +63,12 @@ m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESH void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) { - m_doubleDispatch[proxyType0][proxyType1] = createFunc; + m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc; +} + +void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) +{ + m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc; } btCollisionDispatcher::~btCollisionDispatcher() @@ -84,14 +91,10 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObj btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold()); - void* mem = 0; - - if (m_persistentManifoldPoolAllocator->getFreeCount()) - { - mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); - } else + void* mem = m_persistentManifoldPoolAllocator->allocate( sizeof( btPersistentManifold ) ); + if (NULL == mem) { - //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert. + //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert. if ((m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION)==0) { mem = btAlignedAlloc(sizeof(btPersistentManifold),16); @@ -142,14 +145,23 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) -btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold) + +btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType) { btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = this; ci.m_manifold = sharedManifold; - btCollisionAlgorithm* algo = m_doubleDispatch[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0Wrap,body1Wrap); + btCollisionAlgorithm* algo = 0; + if (algoType == BT_CONTACT_POINT_ALGORITHMS) + { + algo = m_doubleDispatchContactPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap); + } + else + { + algo = m_doubleDispatchClosestPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap); + } return algo; } @@ -228,20 +240,23 @@ class btCollisionPairCallback : public btOverlapCallback 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); + { + BT_PROFILE("processAllOverlappingPairs"); + pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher, dispatchInfo); + } //m_blockedForChanges = false; @@ -249,7 +264,6 @@ void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pa - //by default, Bullet will use this near callback void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo) { @@ -265,7 +279,7 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, //dispatcher will keep algorithms persistent in the collision pair if (!collisionPair.m_algorithm) { - collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap); + collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap,0, BT_CONTACT_POINT_ALGORITHMS); } if (collisionPair.m_algorithm) @@ -293,13 +307,13 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, void* btCollisionDispatcher::allocateCollisionAlgorithm(int size) { - if (m_collisionAlgorithmPoolAllocator->getFreeCount()) - { - return m_collisionAlgorithmPoolAllocator->allocate(size); - } - - //warn user for overflow? - return btAlignedAlloc(static_cast(size), 16); + void* mem = m_collisionAlgorithmPoolAllocator->allocate( size ); + if (NULL == mem) + { + //warn user for overflow? + return btAlignedAlloc(static_cast(size), 16); + } + return mem; } void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h similarity index 92% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index 92696ee54290..b97ee3c1ba6a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -57,7 +57,9 @@ class btCollisionDispatcher : public btDispatcher btPoolAllocator* m_persistentManifoldPoolAllocator; - btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; + btCollisionAlgorithmCreateFunc* m_doubleDispatchContactPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; + + btCollisionAlgorithmCreateFunc* m_doubleDispatchClosestPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; btCollisionConfiguration* m_collisionConfiguration; @@ -84,6 +86,8 @@ class btCollisionDispatcher : public btDispatcher ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); + void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc); + int getNumManifolds() const { return int( m_manifoldsPtr.size()); @@ -115,7 +119,7 @@ class btCollisionDispatcher : public btDispatcher virtual void clearManifold(btPersistentManifold* manifold); - btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold = 0); + btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType); virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1); diff --git a/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp new file mode 100644 index 000000000000..075860c503d4 --- /dev/null +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.cpp @@ -0,0 +1,164 @@ +/* +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 "btCollisionDispatcherMt.h" +#include "LinearMath/btQuickprof.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" +#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" + + +btCollisionDispatcherMt::btCollisionDispatcherMt( btCollisionConfiguration* config, int grainSize ) + : btCollisionDispatcher( config ) +{ + m_batchUpdating = false; + m_grainSize = grainSize; // iterations per task +} + + +btPersistentManifold* btCollisionDispatcherMt::getNewManifold( const btCollisionObject* body0, const btCollisionObject* body1 ) +{ + //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance) + + btScalar contactBreakingThreshold = ( m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD ) ? + btMin( body0->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ), body1->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ) ) + : gContactBreakingThreshold; + + btScalar contactProcessingThreshold = btMin( body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold() ); + + void* mem = m_persistentManifoldPoolAllocator->allocate( sizeof( btPersistentManifold ) ); + if ( NULL == mem ) + { + //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert. + if ( ( m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION ) == 0 ) + { + mem = btAlignedAlloc( sizeof( btPersistentManifold ), 16 ); + } + else + { + btAssert( 0 ); + //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration + return 0; + } + } + btPersistentManifold* manifold = new( mem ) btPersistentManifold( body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold ); + if ( !m_batchUpdating ) + { + // batch updater will update manifold pointers array after finishing, so + // only need to update array when not batch-updating + //btAssert( !btThreadsAreRunning() ); + manifold->m_index1a = m_manifoldsPtr.size(); + m_manifoldsPtr.push_back( manifold ); + } + + return manifold; +} + +void btCollisionDispatcherMt::releaseManifold( btPersistentManifold* manifold ) +{ + clearManifold( manifold ); + //btAssert( !btThreadsAreRunning() ); + if ( !m_batchUpdating ) + { + // batch updater will update manifold pointers array after finishing, so + // only need to update array when not batch-updating + 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 ); + } +} + +struct CollisionDispatcherUpdater : public btIParallelForBody +{ + btBroadphasePair* mPairArray; + btNearCallback mCallback; + btCollisionDispatcher* mDispatcher; + const btDispatcherInfo* mInfo; + + CollisionDispatcherUpdater() + { + mPairArray = NULL; + mCallback = NULL; + mDispatcher = NULL; + mInfo = NULL; + } + void forLoop( int iBegin, int iEnd ) const + { + for ( int i = iBegin; i < iEnd; ++i ) + { + btBroadphasePair* pair = &mPairArray[ i ]; + mCallback( *pair, *mDispatcher, *mInfo ); + } + } +}; + + +void btCollisionDispatcherMt::dispatchAllCollisionPairs( btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher ) +{ + int pairCount = pairCache->getNumOverlappingPairs(); + if ( pairCount == 0 ) + { + return; + } + CollisionDispatcherUpdater updater; + updater.mCallback = getNearCallback(); + updater.mPairArray = pairCache->getOverlappingPairArrayPtr(); + updater.mDispatcher = this; + updater.mInfo = &info; + + m_batchUpdating = true; + btParallelFor( 0, pairCount, m_grainSize, updater ); + m_batchUpdating = false; + + // reconstruct the manifolds array to ensure determinism + m_manifoldsPtr.resizeNoInitialize( 0 ); + + btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr(); + for ( int i = 0; i < pairCount; ++i ) + { + if (btCollisionAlgorithm* algo = pairs[ i ].m_algorithm) + { + algo->getAllContactManifolds( m_manifoldsPtr ); + } + } + + // update the indices (used when releasing manifolds) + for ( int i = 0; i < m_manifoldsPtr.size(); ++i ) + { + m_manifoldsPtr[ i ]->m_index1a = i; + } +} + + diff --git a/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h new file mode 100644 index 000000000000..f1d7eafdc963 --- /dev/null +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h @@ -0,0 +1,39 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_COLLISION_DISPATCHER_MT_H +#define BT_COLLISION_DISPATCHER_MT_H + +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "LinearMath/btThreads.h" + + +class btCollisionDispatcherMt : public btCollisionDispatcher +{ +public: + btCollisionDispatcherMt( btCollisionConfiguration* config, int grainSize = 40 ); + + virtual btPersistentManifold* getNewManifold( const btCollisionObject* body0, const btCollisionObject* body1 ) BT_OVERRIDE; + virtual void releaseManifold( btPersistentManifold* manifold ) BT_OVERRIDE; + + virtual void dispatchAllCollisionPairs( btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher ) BT_OVERRIDE; + +protected: + bool m_batchUpdating; + int m_grainSize; +}; + +#endif //BT_COLLISION_DISPATCHER_MT_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp similarity index 81% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 395df3a550f1..05f96a14bc8d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -16,11 +16,14 @@ subject to the following restrictions: #include "btCollisionObject.h" #include "LinearMath/btSerializer.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" btCollisionObject::btCollisionObject() - : m_anisotropicFriction(1.f,1.f,1.f), - m_hasAnisotropicFriction(false), - m_contactProcessingThreshold(BT_LARGE_FLOAT), + : m_interpolationLinearVelocity(0.f, 0.f, 0.f), + m_interpolationAngularVelocity(0.f, 0.f, 0.f), + m_anisotropicFriction(1.f,1.f,1.f), + m_hasAnisotropicFriction(false), + m_contactProcessingThreshold(BT_LARGE_FLOAT), m_broadphaseHandle(0), m_collisionShape(0), m_extensionPointer(0), @@ -28,13 +31,18 @@ btCollisionObject::btCollisionObject() m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT), m_islandTag1(-1), m_companionId(-1), + m_worldArrayIndex(-1), m_activationState1(1), m_deactivationTime(btScalar(0.)), m_friction(btScalar(0.5)), m_restitution(btScalar(0.)), m_rollingFriction(0.0f), + m_spinningFriction(0.f), + m_contactDamping(.1), + m_contactStiffness(BT_LARGE_FLOAT), m_internalType(CO_COLLISION_OBJECT), m_userObjectPointer(0), + m_userIndex2(-1), m_userIndex(-1), m_hitFraction(btScalar(1.)), m_ccdSweptSphereRadius(btScalar(0.)), @@ -43,6 +51,7 @@ btCollisionObject::btCollisionObject() m_updateRevision(0) { m_worldTransform.setIdentity(); + m_interpolationWorldTransform.setIdentity(); } btCollisionObject::~btCollisionObject() @@ -91,6 +100,8 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali dataOut->m_deactivationTime = m_deactivationTime; dataOut->m_friction = m_friction; dataOut->m_rollingFriction = m_rollingFriction; + dataOut->m_contactDamping = m_contactDamping; + dataOut->m_contactStiffness = m_contactStiffness; dataOut->m_restitution = m_restitution; dataOut->m_internalType = m_internalType; @@ -104,7 +115,18 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius; dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold; dataOut->m_checkCollideWith = m_checkCollideWith; - + if (m_broadphaseHandle) + { + dataOut->m_collisionFilterGroup = m_broadphaseHandle->m_collisionFilterGroup; + dataOut->m_collisionFilterMask = m_broadphaseHandle->m_collisionFilterMask; + dataOut->m_uniqueId = m_broadphaseHandle->m_uniqueId; + } + else + { + dataOut->m_collisionFilterGroup = 0; + dataOut->m_collisionFilterMask = 0; + dataOut->m_uniqueId = -1; + } return btCollisionObjectDataName; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.h similarity index 84% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.h index c68402418f72..135f8a033c37 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -79,13 +79,19 @@ ATTRIBUTE_ALIGNED16(class) btCollisionObject int m_islandTag1; int m_companionId; + int m_worldArrayIndex; // index of object in world's collisionObjects array mutable int m_activationState1; mutable btScalar m_deactivationTime; btScalar m_friction; btScalar m_restitution; - btScalar m_rollingFriction; + btScalar m_rollingFriction;//torsional friction orthogonal to contact normal (useful to stop spheres rolling forever) + btScalar m_spinningFriction; // torsional friction around the contact normal (useful for grasping) + btScalar m_contactDamping; + btScalar m_contactStiffness; + + ///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc. ///do not assign your own m_internalType unless you write a new dynamics object class. @@ -93,8 +99,10 @@ ATTRIBUTE_ALIGNED16(class) btCollisionObject ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer - void* m_userObjectPointer; - + void* m_userObjectPointer; + + int m_userIndex2; + int m_userIndex; ///time of impact calculation @@ -114,6 +122,7 @@ ATTRIBUTE_ALIGNED16(class) btCollisionObject ///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation. int m_updateRevision; + btVector3 m_customDebugColorRGB; public: @@ -127,7 +136,11 @@ ATTRIBUTE_ALIGNED16(class) btCollisionObject CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) CF_CHARACTER_OBJECT = 16, CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing - CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing + CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing + CF_HAS_CONTACT_STIFFNESS_DAMPING = 128, + CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256, + CF_HAS_FRICTION_ANCHOR = 512, + CF_HAS_COLLISION_SOUND_TRIGGER = 1024 }; enum CollisionObjectTypes @@ -316,8 +329,40 @@ ATTRIBUTE_ALIGNED16(class) btCollisionObject { return m_rollingFriction; } - - + void setSpinningFriction(btScalar frict) + { + m_updateRevision++; + m_spinningFriction = frict; + } + btScalar getSpinningFriction() const + { + return m_spinningFriction; + } + void setContactStiffnessAndDamping(btScalar stiffness, btScalar damping) + { + m_updateRevision++; + m_contactStiffness = stiffness; + m_contactDamping = damping; + + m_collisionFlags |=CF_HAS_CONTACT_STIFFNESS_DAMPING; + + //avoid divisions by zero... + if (m_contactStiffness< SIMD_EPSILON) + { + m_contactStiffness = SIMD_EPSILON; + } + } + + btScalar getContactStiffness() const + { + return m_contactStiffness; + } + + btScalar getContactDamping() const + { + return m_contactDamping; + } + ///reserved for Bullet internal usage int getInternalType() const { @@ -415,7 +460,18 @@ ATTRIBUTE_ALIGNED16(class) btCollisionObject m_companionId = id; } - SIMD_FORCE_INLINE btScalar getHitFraction() const + SIMD_FORCE_INLINE int getWorldArrayIndex() const + { + return m_worldArrayIndex; + } + + // only should be called by CollisionWorld + void setWorldArrayIndex(int ix) + { + m_worldArrayIndex = ix; + } + + SIMD_FORCE_INLINE btScalar getHitFraction() const { return m_hitFraction; } @@ -476,6 +532,12 @@ ATTRIBUTE_ALIGNED16(class) btCollisionObject { return m_userIndex; } + + int getUserIndex2() const + { + return m_userIndex2; + } + ///users can point to their objects, userPointer is not used by Bullet void setUserPointer(void* userPointer) { @@ -487,12 +549,37 @@ ATTRIBUTE_ALIGNED16(class) btCollisionObject { m_userIndex = index; } + + void setUserIndex2(int index) + { + m_userIndex2 = index; + } int getUpdateRevisionInternal() const { return m_updateRevision; } + void setCustomDebugColor(const btVector3& colorRGB) + { + m_customDebugColorRGB = colorRGB; + m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; + } + + void removeCustomDebugColor() + { + m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; + } + + bool getCustomDebugColor(btVector3& colorRGB) const + { + bool hasCustomColor = (0!=(m_collisionFlags&CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR)); + if (hasCustomColor) + { + colorRGB = m_customDebugColorRGB; + } + return hasCustomColor; + } inline bool checkCollideWith(const btCollisionObject* co) const { @@ -528,11 +615,12 @@ struct btCollisionObjectDoubleData double m_deactivationTime; double m_friction; double m_rollingFriction; + double m_contactDamping; + double m_contactStiffness; double m_restitution; double m_hitFraction; double m_ccdSweptSphereRadius; double m_ccdMotionThreshold; - int m_hasAnisotropicFriction; int m_collisionFlags; int m_islandTag1; @@ -540,8 +628,9 @@ struct btCollisionObjectDoubleData int m_activationState1; int m_internalType; int m_checkCollideWith; - - char m_padding[4]; + int m_collisionFilterGroup; + int m_collisionFilterMask; + int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 @@ -561,12 +650,12 @@ struct btCollisionObjectFloatData float m_deactivationTime; float m_friction; float m_rollingFriction; - + float m_contactDamping; + float m_contactStiffness; float m_restitution; float m_hitFraction; float m_ccdSweptSphereRadius; float m_ccdMotionThreshold; - int m_hasAnisotropicFriction; int m_collisionFlags; int m_islandTag1; @@ -574,7 +663,9 @@ struct btCollisionObjectFloatData int m_activationState1; int m_internalType; int m_checkCollideWith; - char m_padding[4]; + int m_collisionFilterGroup; + int m_collisionFilterMask; + int m_uniqueId; }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp similarity index 88% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 896cf83a5434..3de8d6995ed0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -21,6 +21,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" //for raycasting #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" @@ -108,14 +109,16 @@ btCollisionWorld::~btCollisionWorld() -void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) +void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) { btAssert(collisionObject); //check that the object isn't already added btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); + btAssert(collisionObject->getWorldArrayIndex() == -1); // do not add the same object to more than one collision world + collisionObject->setWorldArrayIndex(m_collisionObjects.size()); m_collisionObjects.push_back(collisionObject); //calculate new AABB @@ -133,8 +136,7 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho collisionObject, collisionFilterGroup, collisionFilterMask, - m_dispatcher1,0 - )) ; + m_dispatcher1)) ; @@ -195,6 +197,7 @@ void btCollisionWorld::updateAabbs() for ( int i=0;igetWorldArrayIndex() == i); //only update aabb of active objects if (m_forceUpdateAllAabbs || colObj->isActive()) @@ -253,9 +256,25 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) } - //swapremove - m_collisionObjects.remove(collisionObject); - + int iObj = collisionObject->getWorldArrayIndex(); +// btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously? + if (iObj >= 0 && iObj < m_collisionObjects.size()) + { + btAssert(collisionObject == m_collisionObjects[iObj]); + m_collisionObjects.swap(iObj, m_collisionObjects.size()-1); + m_collisionObjects.pop_back(); + if (iObj < m_collisionObjects.size()) + { + m_collisionObjects[iObj]->setWorldArrayIndex(iObj); + } + } + else + { + // slow linear search + //swapremove + m_collisionObjects.remove(collisionObject); + } + collisionObject->setWorldArrayIndex(-1); } @@ -389,6 +408,22 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans,con rcb.m_hitFraction = resultCallback.m_closestHitFraction; triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); } + else if (collisionShape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + ///optimized version for btScaledBvhTriangleMeshShape + btScaledBvhTriangleMeshShape* scaledTriangleMesh = (btScaledBvhTriangleMeshShape*)collisionShape; + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)scaledTriangleMesh->getChildShape(); + + //scale the ray positions + btVector3 scale = scaledTriangleMesh->getLocalScaling(); + btVector3 rayFromLocalScaled = rayFromLocal / scale; + btVector3 rayToLocalScaled = rayToLocal / scale; + + //perform raycast in the underlying btBvhTriangleMeshShape + BridgeTriangleRaycastCallback rcb(rayFromLocalScaled, rayToLocalScaled, &resultCallback, collisionObjectWrap->getCollisionObject(), triangleMesh, colObjWorldTransform); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + triangleMesh->performRaycast(&rcb, rayFromLocalScaled, rayToLocalScaled); + } else { //generic (slower) case @@ -790,23 +825,50 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, } } } else { - ///@todo : use AABB tree or other BVH acceleration structure! if (collisionShape->isCompound()) { - BT_PROFILE("convexSweepCompound"); - const btCompoundShape* compoundShape = static_cast(collisionShape); - int i=0; - for (i=0;igetNumChildShapes();i++) + struct btCompoundLeafCallback : btDbvt::ICollide { - btTransform childTrans = compoundShape->getChildTransform(i); - const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); - btTransform childWorldTrans = colObjWorldTransform * childTrans; - - struct LocalInfoAdder : public ConvexResultCallback { - ConvexResultCallback* m_userCallback; + btCompoundLeafCallback( + const btCollisionObjectWrapper* colObjWrap, + const btConvexShape* castShape, + const btTransform& convexFromTrans, + const btTransform& convexToTrans, + btScalar allowedPenetration, + const btCompoundShape* compoundShape, + const btTransform& colObjWorldTransform, + ConvexResultCallback& resultCallback) + : + m_colObjWrap(colObjWrap), + m_castShape(castShape), + m_convexFromTrans(convexFromTrans), + m_convexToTrans(convexToTrans), + m_allowedPenetration(allowedPenetration), + m_compoundShape(compoundShape), + m_colObjWorldTransform(colObjWorldTransform), + m_resultCallback(resultCallback) { + } + + const btCollisionObjectWrapper* m_colObjWrap; + const btConvexShape* m_castShape; + const btTransform& m_convexFromTrans; + const btTransform& m_convexToTrans; + btScalar m_allowedPenetration; + const btCompoundShape* m_compoundShape; + const btTransform& m_colObjWorldTransform; + ConvexResultCallback& m_resultCallback; + + public: + + void ProcessChild(int index, const btTransform& childTrans, const btCollisionShape* childCollisionShape) + { + btTransform childWorldTrans = m_colObjWorldTransform * childTrans; + + struct LocalInfoAdder : public ConvexResultCallback { + ConvexResultCallback* m_userCallback; int m_i; - LocalInfoAdder (int i, ConvexResultCallback *user) + LocalInfoAdder(int i, ConvexResultCallback *user) : m_userCallback(user), m_i(i) { m_closestHitFraction = m_userCallback->m_closestHitFraction; @@ -815,27 +877,66 @@ void btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape, { return m_userCallback->needsCollision(p); } - virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b) - { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = -1; - shapeInfo.m_triangleIndex = m_i; - if (r.m_localShapeInfo == NULL) - r.m_localShapeInfo = &shapeInfo; - const btScalar result = m_userCallback->addSingleResult(r, b); - m_closestHitFraction = m_userCallback->m_closestHitFraction; - return result; - - } - }; + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& r, bool b) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = -1; + shapeInfo.m_triangleIndex = m_i; + if (r.m_localShapeInfo == NULL) + r.m_localShapeInfo = &shapeInfo; + const btScalar result = m_userCallback->addSingleResult(r, b); + m_closestHitFraction = m_userCallback->m_closestHitFraction; + return result; - LocalInfoAdder my_cb(i, &resultCallback); - - btCollisionObjectWrapper tmpObj(colObjWrap,childCollisionShape,colObjWrap->getCollisionObject(),childWorldTrans,-1,i); + } + }; - objectQuerySingleInternal(castShape, convexFromTrans,convexToTrans, - &tmpObj,my_cb, allowedPenetration); - + LocalInfoAdder my_cb(index, &m_resultCallback); + + btCollisionObjectWrapper tmpObj(m_colObjWrap, childCollisionShape, m_colObjWrap->getCollisionObject(), childWorldTrans, -1, index); + + objectQuerySingleInternal(m_castShape, m_convexFromTrans, m_convexToTrans, &tmpObj, my_cb, m_allowedPenetration); + } + + void Process(const btDbvtNode* leaf) + { + // Processing leaf node + int index = leaf->dataAsInt; + + btTransform childTrans = m_compoundShape->getChildTransform(index); + const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(index); + + ProcessChild(index, childTrans, childCollisionShape); + } + }; + + BT_PROFILE("convexSweepCompound"); + const btCompoundShape* compoundShape = static_cast(collisionShape); + + btVector3 fromLocalAabbMin, fromLocalAabbMax; + btVector3 toLocalAabbMin, toLocalAabbMax; + + castShape->getAabb(colObjWorldTransform.inverse() * convexFromTrans, fromLocalAabbMin, fromLocalAabbMax); + castShape->getAabb(colObjWorldTransform.inverse() * convexToTrans, toLocalAabbMin, toLocalAabbMax); + + fromLocalAabbMin.setMin(toLocalAabbMin); + fromLocalAabbMax.setMax(toLocalAabbMax); + + btCompoundLeafCallback callback(colObjWrap, castShape, convexFromTrans, convexToTrans, + allowedPenetration, compoundShape, colObjWorldTransform, resultCallback); + + const btDbvt* tree = compoundShape->getDynamicAabbTree(); + if (tree) { + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds = btDbvtVolume::FromMM(fromLocalAabbMin, fromLocalAabbMax); + tree->collideTV(tree->m_root, bounds, callback); + } else { + int i; + for (i=0;igetNumChildShapes();i++) + { + const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); + btTransform childTrans = compoundShape->getChildTransform(i); + callback.ProcessChild(i, childTrans, childCollisionShape); + } } } } @@ -1146,7 +1247,7 @@ struct btSingleContactCallback : public btBroadphaseAabbCallback btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1); btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1); - btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1); + btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1,0, BT_CLOSEST_POINT_ALGORITHMS); if (algorithm) { btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback); @@ -1182,10 +1283,11 @@ void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionOb btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1); btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1); - btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB); + btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB, 0, BT_CLOSEST_POINT_ALGORITHMS); if (algorithm) { btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback); + contactPointResult.m_closestPointDistanceThreshold = resultCallback.m_closestDistanceThreshold; //discrete collision detection query algorithm->processCollision(&obA,&obB, getDispatchInfo(),&contactPointResult); @@ -1248,7 +1350,7 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const // Draw a small simplex at the center of the object if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames) { - getDebugDrawer()->drawTransform(worldTransform,1); + getDebugDrawer()->drawTransform(worldTransform,.1); } if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) @@ -1303,10 +1405,10 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const const btCapsuleShape* capsuleShape = static_cast(shape); btScalar radius = capsuleShape->getRadius(); - btScalar extend = capsuleShape->getExtend(); + btScalar halfHeight = capsuleShape->getHalfHeight(); int upAxis = capsuleShape->getUpAxis(); - getDebugDrawer()->drawCapsule(radius, extend, upAxis, worldTransform, color); + getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color); break; } case CONE_SHAPE_PROXYTYPE: @@ -1429,6 +1531,8 @@ void btCollisionWorld::debugDrawWorld() { if (getDebugDrawer()) { + getDebugDrawer()->clearLines(); + btIDebugDraw::DefaultColors defaultColors = getDebugDrawer()->getDefaultColors(); if ( getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) @@ -1486,6 +1590,8 @@ void btCollisionWorld::debugDrawWorld() } }; + colObj->getCustomDebugColor(color); + debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); } if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) @@ -1540,7 +1646,7 @@ void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) for (i=0;igetInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)) + if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) { colObj->serializeSingleObject(serializer); } @@ -1548,12 +1654,36 @@ void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) } + +void btCollisionWorld::serializeContactManifolds(btSerializer* serializer) +{ + if (serializer->getSerializationFlags() & BT_SERIALIZE_CONTACT_MANIFOLDS) + { + int numManifolds = getDispatcher()->getNumManifolds(); + for (int i = 0; i < numManifolds; i++) + { + const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i]; + //don't serialize empty manifolds, they just take space + //(may have to do it anyway if it destroys determinism) + if (manifold->getNumContacts() == 0) + continue; + + btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1); + const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk, structType, BT_CONTACTMANIFOLD_CODE, (void*)manifold); + } + } +} + + void btCollisionWorld::serialize(btSerializer* serializer) { serializer->startSerialization(); serializeCollisionObjects(serializer); + + serializeContactManifolds(serializer); serializer->finishSerialization(); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h similarity index 97% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index ec40c969f51c..fa8803906f25 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -107,6 +107,9 @@ class btCollisionWorld void serializeCollisionObjects(btSerializer* serializer); + void serializeContactManifolds(btSerializer* serializer); + + public: //this constructor doesn't own the dispatcher and paircache/broadphase @@ -205,8 +208,8 @@ class btCollisionWorld { btScalar m_closestHitFraction; const btCollisionObject* m_collisionObject; - short int m_collisionFilterGroup; - short int m_collisionFilterMask; + int m_collisionFilterGroup; + int m_collisionFilterMask; //@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback.h. Apply any of the EFlags defined there on m_flags here to invoke. unsigned int m_flags; @@ -340,8 +343,8 @@ class btCollisionWorld struct ConvexResultCallback { btScalar m_closestHitFraction; - short int m_collisionFilterGroup; - short int m_collisionFilterMask; + int m_collisionFilterGroup; + int m_collisionFilterMask; ConvexResultCallback() :m_closestHitFraction(btScalar(1.)), @@ -410,12 +413,14 @@ class btCollisionWorld ///ContactResultCallback is used to report contact points struct ContactResultCallback { - short int m_collisionFilterGroup; - short int m_collisionFilterMask; - + int m_collisionFilterGroup; + int m_collisionFilterMask; + btScalar m_closestDistanceThreshold; + ContactResultCallback() :m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), - m_collisionFilterMask(btBroadphaseProxy::AllFilter) + m_collisionFilterMask(btBroadphaseProxy::AllFilter), + m_closestDistanceThreshold(0) { } @@ -481,7 +486,7 @@ class btCollisionWorld const btCollisionObjectWrapper* colObjWrap, ConvexResultCallback& resultCallback, btScalar allowedPenetration); - virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); + virtual void addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter); btCollisionObjectArray& getCollisionObjectArray() { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp similarity index 99% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp index 57eb81703e76..f2b083780864 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.cpp @@ -579,10 +579,14 @@ btCollisionShape* btCollisionWorldImporter::convertCollisionShape( btCollisionS btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData; btCompoundShape* compoundShape = createCompoundShape(); + //btCompoundShapeChildData* childShapeDataArray = &compoundData->m_childShapePtr[0]; + btAlignedObjectArray childShapes; for (int i=0;im_numChildShapes;i++) { + //btCompoundShapeChildData* ptr = &compoundData->m_childShapePtr[i]; + btCollisionShapeData* cd = compoundData->m_childShapePtr[i].m_childShape; btCollisionShape* childShape = convertCollisionShape(cd); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h similarity index 99% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h index 9a6d16fbea7c..81c61427267f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorldImporter.h @@ -124,7 +124,6 @@ class btCollisionWorldImporter btCollisionShape* getCollisionShapeByIndex(int index); int getNumRigidBodies() const; btCollisionObject* getRigidBodyByIndex(int index) const; - int getNumConstraints() const; int getNumBvhs() const; btOptimizedBvh* getBvhByIndex(int index) const; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp similarity index 89% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 589a7f55f2b8..9d96ef8ad2f0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -65,7 +65,13 @@ void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionO const btCollisionShape* childShape = compoundShape->getChildShape(i); btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully) - m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold); + m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + + + btAlignedObjectArray m_childCollisionAlgorithmsContact; + btAlignedObjectArray m_childCollisionAlgorithmsClosestPoints; + + } } } @@ -128,8 +134,14 @@ struct btCompoundLeafCallback : btDbvt::ICollide btTransform newChildWorldTrans = orgTrans*childTrans ; //perform an AABB check first - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + btVector3 aabbMin0,aabbMax0; childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); + + btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold); + aabbMin0 -= extendAabb; + aabbMax0 += extendAabb; + + btVector3 aabbMin1, aabbMax1; m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); if (gCompoundChildShapePairCallback) @@ -142,12 +154,22 @@ struct btCompoundLeafCallback : btDbvt::ICollide { btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index); + + btCollisionAlgorithm* algo = 0; - - //the contactpoint is still projected back using the original inverted worldtrans - if (!m_childCollisionAlgorithms[index]) - m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold); - + if (m_resultOut->m_closestPointDistanceThreshold > 0) + { + algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); + } + else + { + //the contactpoint is still projected back using the original inverted worldtrans + if (!m_childCollisionAlgorithms[index]) + { + m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + } + algo = m_childCollisionAlgorithms[index]; + } const btCollisionObjectWrapper* tmpWrap = 0; @@ -164,8 +186,7 @@ struct btCompoundLeafCallback : btDbvt::ICollide m_resultOut->setShapeIdentifiersB(-1,index); } - - m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut); + algo->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut); #if 0 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) @@ -234,7 +255,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap if (m_childCollisionAlgorithms.size()==0) return; - + const btDbvt* tree = compoundShape->getDynamicAabbTree(); //use a dynamic aabb tree to cull potential child-overlaps btCompoundLeafCallback callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold); @@ -244,7 +265,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm { int i; - btManifoldArray manifoldArray; + manifoldArray.resize(0); for (i=0;igetWorldTransform().inverse() * otherObjWrap->getWorldTransform(); otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax); + btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold); + localAabbMin -= extraExtends; + localAabbMax += extraExtends; 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); + tree->collideTVNoStackAlloc(tree->m_root,bounds,stack2,callback); } else { @@ -291,7 +315,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap //iterate over all children, perform an AABB check inside ProcessChildShape int numChildren = m_childCollisionAlgorithms.size(); int i; - btManifoldArray manifoldArray; + manifoldArray.resize(0); const btCollisionShape* childShape = 0; btTransform orgTrans; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h similarity index 97% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index 7d792c18d348..d2086fbc0260 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -26,6 +26,7 @@ class btDispatcher; #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "btCollisionCreateFunc.h" #include "LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/BroadphaseCollision/btDbvt.h" class btDispatcher; class btCollisionObject; @@ -36,6 +37,9 @@ extern btShapePairCallback gCompoundChildShapePairCallback; /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm { + btNodeStack stack2; + btManifoldArray manifoldArray; + protected: btAlignedObjectArray m_childCollisionAlgorithms; bool m_isSwapped; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp similarity index 85% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp index 1d64d84b87b7..108942b88760 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -15,6 +15,7 @@ subject to the following restrictions: */ #include "btCompoundCompoundCollisionAlgorithm.h" +#include "LinearMath/btQuickprof.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/BroadphaseCollision/btDbvt.h" @@ -23,6 +24,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btManifoldResult.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" +//USE_LOCAL_STACK will avoid most (often all) dynamic memory allocations due to resizing in processCollision and MycollideTT +#define USE_LOCAL_STACK 1 btShapePairCallback gCompoundCompoundChildShapePairCallback = 0; @@ -124,6 +127,7 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1) { + BT_PROFILE("btCompoundCompoundLeafCallback::Process"); m_numOverlapPairs++; @@ -159,6 +163,11 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); + btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold); + + aabbMin0 -= thresholdVec; + aabbMax0 += thresholdVec; + if (gCompoundCompoundChildShapePairCallback) { if (!gCompoundCompoundChildShapePairCallback(childShape0,childShape1)) @@ -174,17 +183,24 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1); btCollisionAlgorithm* colAlgo = 0; - - if (pair) + if (m_resultOut->m_closestPointDistanceThreshold > 0) { - colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; - - } else + colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS); + } + else { - colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold); - pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1); - btAssert(pair); - pair->m_userPointer = colAlgo; + if (pair) + { + colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; + + } + else + { + colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1); + btAssert(pair); + pair->m_userPointer = colAlgo; + } } btAssert(colAlgo); @@ -215,10 +231,12 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, const btTransform& xform) + const btDbvtAabbMm& b, const btTransform& xform, btScalar distanceThreshold) { btVector3 newmin,newmax; btTransformAabb(b.Mins(),b.Maxs(),0.f,xform,newmin,newmax); + newmin -= btVector3(distanceThreshold, distanceThreshold, distanceThreshold); + newmax += btVector3(distanceThreshold, distanceThreshold, distanceThreshold); btDbvtAabbMm newb = btDbvtAabbMm::FromMM(newmin,newmax); return Intersect(a,newb); } @@ -227,7 +245,7 @@ static DBVT_INLINE bool MyIntersect( const btDbvtAabbMm& a, static inline void MycollideTT( const btDbvtNode* root0, const btDbvtNode* root1, const btTransform& xform, - btCompoundCompoundLeafCallback* callback) + btCompoundCompoundLeafCallback* callback, btScalar distanceThreshold) { if(root0&&root1) @@ -235,11 +253,16 @@ static inline void MycollideTT( const btDbvtNode* root0, int depth=1; int treshold=btDbvt::DOUBLE_STACKSIZE-4; btAlignedObjectArray stkStack; +#ifdef USE_LOCAL_STACK + ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]); + stkStack.initializeFromBuffer(&localStack,btDbvt::DOUBLE_STACKSIZE,btDbvt::DOUBLE_STACKSIZE); +#else stkStack.resize(btDbvt::DOUBLE_STACKSIZE); +#endif stkStack[0]=btDbvt::sStkNN(root0,root1); do { btDbvt::sStkNN p=stkStack[--depth]; - if(MyIntersect(p.a->volume,p.b->volume,xform)) + if(MyIntersect(p.a->volume,p.b->volume,xform, distanceThreshold)) { if(depth>treshold) { @@ -313,6 +336,10 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb { int i; btManifoldArray manifoldArray; +#ifdef USE_LOCAL_STACK + btPersistentManifold localManifolds[4]; + manifoldArray.initializeFromBuffer(&localManifolds,0,4); +#endif btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray(); for (i=0;igetWorldTransform().inverse()*col1ObjWrap->getWorldTransform(); - MycollideTT(tree0->m_root,tree1->m_root,xform,&callback); + MycollideTT(tree0->m_root,tree1->m_root,xform,&callback, resultOut->m_closestPointDistanceThreshold); //printf("#compound-compound child/leaf overlap =%d \r",callback.m_numOverlapPairs); @@ -369,34 +396,29 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer; { - btTransform orgTrans0; const btCollisionShape* childShape0 = 0; btTransform newChildWorldTrans0; - btTransform orgInterpolationTrans0; childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA); - orgTrans0 = col0ObjWrap->getWorldTransform(); - orgInterpolationTrans0 = col0ObjWrap->getWorldTransform(); const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA); - newChildWorldTrans0 = orgTrans0*childTrans0 ; + newChildWorldTrans0 = col0ObjWrap->getWorldTransform()*childTrans0 ; childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); } - + btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold); + aabbMin0 -= thresholdVec; + aabbMax0 += thresholdVec; { - btTransform orgInterpolationTrans1; const btCollisionShape* childShape1 = 0; - btTransform orgTrans1; btTransform newChildWorldTrans1; childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB); - orgTrans1 = col1ObjWrap->getWorldTransform(); - orgInterpolationTrans1 = col1ObjWrap->getWorldTransform(); const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB); - newChildWorldTrans1 = orgTrans1*childTrans1 ; + newChildWorldTrans1 = col1ObjWrap->getWorldTransform()*childTrans1 ; childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); } - + aabbMin1 -= thresholdVec; + aabbMax1 += thresholdVec; if (!TestAabbAgainstAabb2(aabbMin0,aabbMax0,aabbMin1,aabbMax1)) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h similarity index 95% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h index 06a762f209d3..f29f7a709a49 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h @@ -33,8 +33,6 @@ class btDispatcher; class btCollisionObject; class btCollisionShape; -typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCollisionShape* pShape1); -extern btShapePairCallback gCompoundCompoundChildShapePairCallback; /// btCompoundCompoundCollisionAlgorithm supports collision between two btCompoundCollisionShape shapes class btCompoundCompoundCollisionAlgorithm : public btCompoundCollisionAlgorithm diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp similarity index 71% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 912a52855682..d8cbe9614295 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -15,6 +15,7 @@ subject to the following restrictions: #include "btConvexConcaveCollisionAlgorithm.h" +#include "LinearMath/btQuickprof.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -26,11 +27,12 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" +#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h" btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_isSwapped(isSwapped), -m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped) +m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped), +m_isSwapped(isSwapped) { } @@ -79,6 +81,7 @@ void btConvexTriangleCallback::clearCache() void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) { + BT_PROFILE("btConvexTriangleCallback::processTriangle"); if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax)) { @@ -116,8 +119,16 @@ partId, int triangleIndex) btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform? - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr); - + btCollisionAlgorithm* colAlgo = 0; + + if (m_resultOut->m_closestPointDistanceThreshold > 0) + { + colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); + } + else + { + colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS); + } const btCollisionObjectWrapper* tmpWrap = 0; if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) @@ -142,7 +153,7 @@ partId, int triangleIndex) { m_resultOut->setBody1Wrap(tmpWrap); } - + colAlgo->~btCollisionAlgorithm(); @@ -153,7 +164,7 @@ partId, int triangleIndex) -void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut) +void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut) { m_convexBodyWrap = convexBodyWrap; m_triBodyWrap = triBodyWrap; @@ -167,13 +178,14 @@ void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform(); const btCollisionShape* convexShape = static_cast(m_convexBodyWrap->getCollisionShape()); //CollisionShape* triangleShape = static_cast(triBody->m_collisionShape); - convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); - btScalar extraMargin = collisionMarginTriangle; - btVector3 extra(extraMargin,extraMargin,extraMargin); + convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax); + btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold; + + btVector3 extra(extraMargin, extraMargin, extraMargin); m_aabbMax += extra; m_aabbMin -= extra; - + } void btConvexConcaveCollisionAlgorithm::clearCache() @@ -182,35 +194,99 @@ void btConvexConcaveCollisionAlgorithm::clearCache() } -void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { - - + BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision"); + const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap; const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap; if (triBodyWrap->getCollisionShape()->isConcave()) { + if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE) + { + btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape(); + if (convexBodyWrap->getCollisionShape()->isConvex()) + { + btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape(); + btAlignedObjectArray queryVertices; + + if (convex->isPolyhedral()) + { + btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; + for (int v = 0; v < poly->getNumVertices(); v++) + { + btVector3 vtx; + poly->getVertex(v, vtx); + queryVertices.push_back(vtx); + } + } + btScalar maxDist = SIMD_EPSILON; + + if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) + { + queryVertices.push_back(btVector3(0, 0, 0)); + btSphereShape* sphere = (btSphereShape*)convex; + maxDist = sphere->getRadius() + SIMD_EPSILON; + + } + if (queryVertices.size()) + { + resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); + //m_btConvexTriangleCallback.m_manifoldPtr->clearManifold(); + + btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; + for (int v = 0; v < queryVertices.size(); v++) + { + const btVector3& vtx = queryVertices[v]; + btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform()*vtx; + btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace); + + btVector3 normalLocal; + btScalar dist; + if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal)) + { + if (dist <= maxDist) + { + normalLocal.safeNormalize(); + btVector3 normal = triBodyWrap->getWorldTransform().getBasis()*normalLocal; + + if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) + { + btSphereShape* sphere = (btSphereShape*)convex; + dist -= sphere->getRadius(); + vtxWorldSpace -= sphere->getRadius()*normal; + + } + resultOut->addContactPoint(normal,vtxWorldSpace-normal*dist, dist); + } + } + } + resultOut->refreshContactPoints(); + } - - const btConcaveShape* concaveShape = static_cast( triBodyWrap->getCollisionShape()); - - if (convexBodyWrap->getCollisionShape()->isConvex()) + } + } else { - btScalar collisionMarginTriangle = concaveShape->getMargin(); + const btConcaveShape* concaveShape = static_cast( triBodyWrap->getCollisionShape()); + + if (convexBodyWrap->getCollisionShape()->isConvex()) + { + btScalar collisionMarginTriangle = concaveShape->getMargin(); - resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); - m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut); + resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); + m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut); - m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject()); + m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject()); - concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax()); + concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax()); - resultOut->refreshContactPoints(); + resultOut->refreshContactPoints(); - m_btConvexTriangleCallback.clearWrapperData(); + m_btConvexTriangleCallback.clearWrapperData(); + } } } @@ -265,6 +341,7 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { + BT_PROFILE("processTriangle"); (void)partId; (void)triangleIndex; //do a swept sphere for now diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h similarity index 94% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h index e90d06eb191b..93d842ef5014 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -26,14 +26,16 @@ class btDispatcher; #include "btCollisionCreateFunc.h" ///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called. -class btConvexTriangleCallback : public btTriangleCallback +ATTRIBUTE_ALIGNED16(class) btConvexTriangleCallback : public btTriangleCallback { - const btCollisionObjectWrapper* m_convexBodyWrap; - const btCollisionObjectWrapper* m_triBodyWrap; btVector3 m_aabbMin; btVector3 m_aabbMax ; + const btCollisionObjectWrapper* m_convexBodyWrap; + const btCollisionObjectWrapper* m_triBodyWrap; + + btManifoldResult* m_resultOut; btDispatcher* m_dispatcher; @@ -41,6 +43,8 @@ class btConvexTriangleCallback : public btTriangleCallback btScalar m_collisionMarginTriangle; public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + int m_triangleCount; btPersistentManifold* m_manifoldPtr; @@ -75,17 +79,19 @@ int m_triangleCount; /// btConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes. -class btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm +ATTRIBUTE_ALIGNED16(class) btConvexConcaveCollisionAlgorithm : public btActivatingCollisionAlgorithm { - bool m_isSwapped; - btConvexTriangleCallback m_btConvexTriangleCallback; + bool m_isSwapped; + public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped); virtual ~btConvexConcaveCollisionAlgorithm(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp similarity index 78% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 7f2722aa463c..3e8bc6e426e7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: ///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 +//#define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1 //#define ZERO_MARGIN #include "btConvexConvexAlgorithm.h" @@ -28,6 +28,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btCapsuleShape.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" @@ -179,11 +180,10 @@ static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance( -btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) +btConvexConvexAlgorithm::CreateFunc::CreateFunc(btConvexPenetrationDepthSolver* pdSolver) { m_numPerturbationIterations = 0; m_minimumPointsPerturbationThreshold = 3; - m_simplexSolver = simplexSolver; m_pdSolver = pdSolver; } @@ -191,9 +191,8 @@ btConvexConvexAlgorithm::CreateFunc::~CreateFunc() { } -btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold) +btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold) : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), -m_simplexSolver(simplexSolver), m_pdSolver(pdSolver), m_ownManifold (false), m_manifoldPtr(mf), @@ -261,7 +260,7 @@ struct btPerturbedContactResult : public btManifoldResult btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth; endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg); newDepth = (endPt - pointInWorld).dot(normalOnBInWorld); - startPt = endPt+normalOnBInWorld*newDepth; + startPt = endPt - normalOnBInWorld*newDepth; } else { endPt = pointInWorld + normalOnBInWorld*orgDepth; @@ -312,10 +311,10 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) { + //m_manifoldPtr->clearManifold(); + btCapsuleShape* capsuleA = (btCapsuleShape*) min0; btCapsuleShape* capsuleB = (btCapsuleShape*) min1; - // btVector3 localScalingA = capsuleA->getLocalScaling(); - // btVector3 localScalingB = capsuleB->getLocalScaling(); btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); @@ -331,6 +330,50 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* resultOut->refreshContactPoints(); return; } + + if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == SPHERE_SHAPE_PROXYTYPE)) + { + //m_manifoldPtr->clearManifold(); + + btCapsuleShape* capsuleA = (btCapsuleShape*) min0; + btSphereShape* capsuleB = (btSphereShape*) min1; + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(), + 0.,capsuleB->getRadius(),capsuleA->getUpAxis(),1, + body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); + + if (dist=(SIMD_EPSILON*SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); + } + resultOut->refreshContactPoints(); + return; + } + + if ((min0->getShapeType() == SPHERE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE)) + { + //m_manifoldPtr->clearManifold(); + + btSphereShape* capsuleA = (btSphereShape*) min0; + btCapsuleShape* capsuleB = (btCapsuleShape*) min1; + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + + btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,0.,capsuleA->getRadius(), + capsuleB->getHalfHeight(),capsuleB->getRadius(),1,capsuleB->getUpAxis(), + body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold); + + if (dist=(SIMD_EPSILON*SIMD_EPSILON)); + resultOut->addContactPoint(normalOnB,pointOnBWorld,dist); + } + resultOut->refreshContactPoints(); + return; + } #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER @@ -349,8 +392,8 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* btGjkPairDetector::ClosestPointInput input; - - btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); + btVoronoiSimplexSolver simplexSolver; + btGjkPairDetector gjkPairDetector( min0, min1, &simplexSolver, m_pdSolver ); //TODO: if (dispatchInfo.m_useContinuous) gjkPairDetector.setMinkowskiA(min0); gjkPairDetector.setMinkowskiB(min1); @@ -367,7 +410,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); //} else //{ - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold()+resultOut->m_closestPointDistanceThreshold; // } input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; @@ -400,10 +443,26 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result { + btVector3 m_normalOnBInWorld; + btVector3 m_pointInWorld; + btScalar m_depth; + bool m_hasContact; + + + btDummyResult() + : m_hasContact(false) + { + } + + virtual void setShapeIdentifiersA(int partId0,int index0){} virtual void setShapeIdentifiersB(int partId1,int index1){} virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { + m_hasContact = true; + m_normalOnBInWorld = normalOnBInWorld; + m_pointInWorld = pointInWorld; + m_depth = depth; } }; @@ -503,9 +562,11 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + worldVertsB1.resize(0); btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut); + body1Wrap->getWorldTransform(), minDist-threshold, threshold, worldVertsB1,worldVertsB2, + *resultOut); } if (m_ownManifold) @@ -516,15 +577,18 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* } else { + + //we can also deal with convex versus triangle (without connectivity data) - if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) + if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) { - btVertexArray vertices; + + btVertexArray worldSpaceVertices; btTriangleShape* tri = (btTriangleShape*)polyhedronB; - vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]); - vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]); - vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]); + worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]); + worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]); + worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]); //tri->initializePolyhedralFeatures(); @@ -535,17 +599,99 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* btScalar maxDist = threshold; bool foundSepAxis = false; - if (0) + bool useSatSepNormal = true; + + if (useSatSepNormal) { - polyhedronB->initializePolyhedralFeatures(); +#if 0 + if (0) + { + //initializePolyhedralFeatures performs a convex hull computation, not needed for a single triangle + polyhedronB->initializePolyhedralFeatures(); + } else +#endif + { + + btVector3 uniqueEdges[3] = {tri->m_vertices1[1]-tri->m_vertices1[0], + tri->m_vertices1[2]-tri->m_vertices1[1], + tri->m_vertices1[0]-tri->m_vertices1[2]}; + + uniqueEdges[0].normalize(); + uniqueEdges[1].normalize(); + uniqueEdges[2].normalize(); + + btConvexPolyhedron polyhedron; + polyhedron.m_vertices.push_back(tri->m_vertices1[2]); + polyhedron.m_vertices.push_back(tri->m_vertices1[0]); + polyhedron.m_vertices.push_back(tri->m_vertices1[1]); + + + { + btFace combinedFaceA; + combinedFaceA.m_indices.push_back(0); + combinedFaceA.m_indices.push_back(1); + combinedFaceA.m_indices.push_back(2); + btVector3 faceNormal = uniqueEdges[0].cross(uniqueEdges[1]); + faceNormal.normalize(); + btScalar planeEq=1e30f; + for (int v=0;vm_vertices1[combinedFaceA.m_indices[v]].dot(faceNormal); + if (planeEq>eq) + { + planeEq=eq; + } + } + combinedFaceA.m_plane[0] = faceNormal[0]; + combinedFaceA.m_plane[1] = faceNormal[1]; + combinedFaceA.m_plane[2] = faceNormal[2]; + combinedFaceA.m_plane[3] = -planeEq; + polyhedron.m_faces.push_back(combinedFaceA); + } + { + btFace combinedFaceB; + combinedFaceB.m_indices.push_back(0); + combinedFaceB.m_indices.push_back(2); + combinedFaceB.m_indices.push_back(1); + btVector3 faceNormal = -uniqueEdges[0].cross(uniqueEdges[1]); + faceNormal.normalize(); + btScalar planeEq=1e30f; + for (int v=0;vm_vertices1[combinedFaceB.m_indices[v]].dot(faceNormal); + if (planeEq>eq) + { + planeEq=eq; + } + } + + combinedFaceB.m_plane[0] = faceNormal[0]; + combinedFaceB.m_plane[1] = faceNormal[1]; + combinedFaceB.m_plane[2] = faceNormal[2]; + combinedFaceB.m_plane[3] = -planeEq; + polyhedron.m_faces.push_back(combinedFaceB); + } + + + polyhedron.m_uniqueEdges.push_back(uniqueEdges[0]); + polyhedron.m_uniqueEdges.push_back(uniqueEdges[1]); + polyhedron.m_uniqueEdges.push_back(uniqueEdges[2]); + polyhedron.initialize2(); + + polyhedronB->setPolyhedralFeatures(polyhedron); + } + + + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( - *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), - sepNormalWorldSpace,*resultOut); + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), + body1Wrap->getWorldTransform(), + sepNormalWorldSpace,*resultOut); // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); - } else + } + else { #ifdef ZERO_MARGIN gjkPairDetector.setIgnoreMargin(true); @@ -554,6 +700,24 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); #endif//ZERO_MARGIN + if (dummy.m_hasContact && dummy.m_depth<0) + { + + if (foundSepAxis) + { + if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace)<0.99) + { + printf("?\n"); + } + } else + { + printf("!\n"); + } + sepNormalWorldSpace.setValue(0,0,1);// = dummy.m_normalOnBInWorld; + //minDist = dummy.m_depth; + foundSepAxis = true; + } +#if 0 btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if (l2>SIMD_EPSILON) { @@ -563,13 +727,15 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); foundSepAxis = true; } +#endif } if (foundSepAxis) { + worldVertsB2.resize(0); btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut); + body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2,minDist-threshold, maxDist, *resultOut); } @@ -580,6 +746,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* return; } + } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h similarity index 89% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index 51db0c6548dc..cd75ba12d7f9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -24,6 +24,7 @@ subject to the following restrictions: #include "btCollisionCreateFunc.h" #include "btCollisionDispatcher.h" #include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil +#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h" class btConvexPenetrationDepthSolver; @@ -42,9 +43,10 @@ class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm #ifdef USE_SEPDISTANCE_UTIL2 btConvexSeparatingDistanceUtil m_sepDistance; #endif - btSimplexSolverInterface* m_simplexSolver; btConvexPenetrationDepthSolver* m_pdSolver; + btVertexArray worldVertsB1; + btVertexArray worldVertsB2; bool m_ownManifold; btPersistentManifold* m_manifoldPtr; @@ -59,7 +61,7 @@ class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm public: - btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); + btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); virtual ~btConvexConvexAlgorithm(); @@ -87,18 +89,17 @@ class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm { btConvexPenetrationDepthSolver* m_pdSolver; - btSimplexSolverInterface* m_simplexSolver; int m_numPerturbationIterations; int m_minimumPointsPerturbationThreshold; - CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); + CreateFunc(btConvexPenetrationDepthSolver* pdSolver); virtual ~CreateFunc(); virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) { void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm)); - return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0Wrap,body1Wrap,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); } }; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp similarity index 85% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index d42f00a637ff..f6e4e57b0a33 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -44,9 +44,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault //btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(btStackAlloc* stackAlloc,btPoolAllocator* persistentManifoldPool,btPoolAllocator* collisionAlgorithmPool) { - void* mem = btAlignedAlloc(sizeof(btVoronoiSimplexSolver),16); - m_simplexSolver = new (mem)btVoronoiSimplexSolver(); - + void* mem = NULL; if (constructionInfo.m_useEpaPenetrationAlgorithm) { mem = btAlignedAlloc(sizeof(btGjkEpaPenetrationDepthSolver),16); @@ -59,7 +57,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault //default CreationFunctions, filling the m_doubleDispatch table mem = btAlignedAlloc(sizeof(btConvexConvexAlgorithm::CreateFunc),16); - m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_simplexSolver,m_pdSolver); + m_convexConvexCreateFunc = new(mem) btConvexConvexAlgorithm::CreateFunc(m_pdSolver); mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); m_convexConcaveCreateFunc = new (mem)btConvexConcaveCollisionAlgorithm::CreateFunc; mem = btAlignedAlloc(sizeof(btConvexConcaveCollisionAlgorithm::CreateFunc),16); @@ -123,6 +121,7 @@ btDefaultCollisionConfiguration::btDefaultCollisionConfiguration(const btDefault m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),constructionInfo.m_defaultMaxPersistentManifoldPoolSize); } + collisionAlgorithmMaxElementSize = (collisionAlgorithmMaxElementSize+16)&0xffffffffffff0; if (constructionInfo.m_collisionAlgorithmPool) { m_ownsCollisionAlgorithmPool = false; @@ -192,9 +191,6 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() m_planeConvexCF->~btCollisionAlgorithmCreateFunc(); btAlignedFree( m_planeConvexCF); - m_simplexSolver->~btVoronoiSimplexSolver(); - btAlignedFree(m_simplexSolver); - m_pdSolver->~btConvexPenetrationDepthSolver(); btAlignedFree(m_pdSolver); @@ -202,6 +198,86 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() } +btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(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 (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) && btBroadphaseProxy::isCompound(proxyType1)) + { + return m_compoundCompoundCreateFunc; + } + + if (btBroadphaseProxy::isCompound(proxyType0)) + { + return m_compoundCreateFunc; + } + else + { + if (btBroadphaseProxy::isCompound(proxyType1)) + { + return m_swappedCompoundCreateFunc; + } + } + + //failed to find an algorithm + return m_emptyCreateFunc; + +} btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h similarity index 96% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 2078420e1989..17c7596cffb4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -60,8 +60,7 @@ class btDefaultCollisionConfiguration : public btCollisionConfiguration btPoolAllocator* m_collisionAlgorithmPool; bool m_ownsCollisionAlgorithmPool; - //default simplex/penetration depth solvers - btVoronoiSimplexSolver* m_simplexSolver; + //default penetration depth solver btConvexPenetrationDepthSolver* m_pdSolver; //default CreationFunctions, filling the m_doubleDispatch table @@ -102,14 +101,10 @@ class btDefaultCollisionConfiguration : public btCollisionConfiguration } - virtual btVoronoiSimplexSolver* getSimplexSolver() - { - return m_simplexSolver; - } - - virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); + virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1); + ///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm. ///By default, this feature is disabled for best performance. ///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature. diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btGhostObject.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btGhostObject.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btGhostObject.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btGhostObject.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btGhostObject.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp similarity index 97% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp index 8c8a7c3c1e3b..8271981b29d6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp @@ -20,11 +20,12 @@ subject to the following restrictions: #include +#ifdef BT_DEBUG_COLLISION_PAIRS int gOverlappingSimplePairs = 0; int gRemoveSimplePairs =0; int gAddedSimplePairs =0; int gFindSimplePairs =0; - +#endif //BT_DEBUG_COLLISION_PAIRS @@ -61,7 +62,9 @@ void btHashedSimplePairCache::removeAllPairs() btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gFindSimplePairs++; +#endif /*if (indexA > indexB) @@ -172,7 +175,9 @@ btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gRemoveSimplePairs++; +#endif /*if (indexA > indexB) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h similarity index 94% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h index 186964d723e2..318981cda1c8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -43,12 +43,12 @@ struct btSimplePair typedef btAlignedObjectArray btSimplePairArray; - +#ifdef BT_DEBUG_COLLISION_PAIRS extern int gOverlappingSimplePairs; extern int gRemoveSimplePairs; extern int gAddedSimplePairs; extern int gFindSimplePairs; - +#endif //BT_DEBUG_COLLISION_PAIRS @@ -75,7 +75,9 @@ class btHashedSimplePairCache // no new pair is created and the old one is returned. virtual btSimplePair* addOverlappingPair(int indexA,int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gAddedSimplePairs++; +#endif return internalAddPair(indexA,indexB); } @@ -123,9 +125,9 @@ class btHashedSimplePairCache - SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) + SIMD_FORCE_INLINE unsigned int getHash(unsigned int indexA, unsigned int indexB) { - int key = static_cast(((unsigned int)indexA) | (((unsigned int)indexB) <<16)); + unsigned int key = indexA | (indexB << 16); // Thomas Wang's hash key += ~(key << 15); @@ -134,7 +136,7 @@ class btHashedSimplePairCache key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); - return static_cast(key); + return key; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp similarity index 57% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 4b2986a00873..23c73c88254c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -22,12 +22,16 @@ subject to the following restrictions: ///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 calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1) +CalculateCombinedCallback gCalculateCombinedRestitutionCallback = &btManifoldResult::calculateCombinedRestitution; +CalculateCombinedCallback gCalculateCombinedFrictionCallback = &btManifoldResult::calculateCombinedFriction; +CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback = &btManifoldResult::calculateCombinedRollingFriction; +CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback = &btManifoldResult::calculateCombinedSpinningFriction; +CalculateCombinedCallback gCalculateCombinedContactDampingCallback = &btManifoldResult::calculateCombinedContactDamping; +CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback = &btManifoldResult::calculateCombinedContactStiffness; + +btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1) { - btScalar friction = body0->getRollingFriction() * body1->getRollingFriction(); + btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction(); const btScalar MAX_FRICTION = btScalar(10.); if (friction < -MAX_FRICTION) @@ -38,6 +42,17 @@ inline btScalar calculateCombinedRollingFriction(const btCollisionObject* body0, } +btScalar btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1) +{ + btScalar friction = body0->getSpinningFriction() * body1->getFriction() + body1->getSpinningFriction() * body0->getFriction(); + + const btScalar MAX_FRICTION = btScalar(10.); + if (friction < -MAX_FRICTION) + friction = -MAX_FRICTION; + if (friction > MAX_FRICTION) + friction = MAX_FRICTION; + return friction; +} ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; btScalar btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1) @@ -58,6 +73,22 @@ btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject* return body0->getRestitution() * body1->getRestitution(); } +btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1) +{ + return body0->getContactDamping() + body1->getContactDamping(); +} + +btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1) +{ + + btScalar s0 = body0->getContactStiffness(); + btScalar s1 = body1->getContactStiffness(); + + btScalar tmp0 = btScalar(1)/s0; + btScalar tmp1 = btScalar(1)/s1; + btScalar combinedStiffness = btScalar(1) / (tmp0+tmp1); + return combinedStiffness; +} btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap) @@ -70,6 +101,7 @@ btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,con m_index0(-1), m_index1(-1) #endif //DEBUG_PART_INDEX + , m_closestPointDistanceThreshold(0) { } @@ -84,6 +116,7 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b return; bool isSwapped = m_manifoldPtr->getBody0() != m_body0Wrap->getCollisionObject(); + bool isNewCollision = m_manifoldPtr->getNumContacts() == 0; btVector3 pointA = pointInWorld + normalOnBInWorld * depth; @@ -106,9 +139,25 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b int insertIndex = m_manifoldPtr->getCacheEntry(newPt); - newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); - newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); - newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_combinedFriction = gCalculateCombinedFrictionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_combinedRestitution = gCalculateCombinedRestitutionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_combinedRollingFriction = gCalculateCombinedRollingFrictionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_combinedSpinningFriction = gCalculateCombinedSpinningFrictionCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + + if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)) + { + newPt.m_combinedContactDamping1 = gCalculateCombinedContactDampingCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_combinedContactStiffness1 = gCalculateCombinedContactStiffnessCallback(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING; + } + + if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR) || + (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR)) + { + newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR; + } + btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2); @@ -150,5 +199,9 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b (*gContactAddedCallback)(m_manifoldPtr->getContactPoint(insertIndex),obj0Wrap,newPt.m_partId0,newPt.m_index0,obj1Wrap,newPt.m_partId1,newPt.m_index1); } + if (gContactStartedCallback && isNewCollision) + { + gContactStartedCallback(m_manifoldPtr); + } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.h similarity index 78% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 977b9a02fc55..12cdafd1b6ae 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -34,6 +34,15 @@ extern ContactAddedCallback gContactAddedCallback; //#define DEBUG_PART_INDEX 1 +/// These callbacks are used to customize the algorith that combine restitution, friction, damping, Stiffness +typedef btScalar (*CalculateCombinedCallback)(const btCollisionObject* body0,const btCollisionObject* body1); + +extern CalculateCombinedCallback gCalculateCombinedRestitutionCallback; +extern CalculateCombinedCallback gCalculateCombinedFrictionCallback; +extern CalculateCombinedCallback gCalculateCombinedRollingFrictionCallback; +extern CalculateCombinedCallback gCalculateCombinedSpinningFrictionCallback; +extern CalculateCombinedCallback gCalculateCombinedContactDampingCallback; +extern CalculateCombinedCallback gCalculateCombinedContactStiffnessCallback; ///btManifoldResult is a helper class to manage contact results. class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result @@ -49,17 +58,19 @@ class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result int m_index0; int m_index1; - + public: btManifoldResult() -#ifdef DEBUG_PART_INDEX : +#ifdef DEBUG_PART_INDEX + m_partId0(-1), m_partId1(-1), m_index0(-1), m_index1(-1) #endif //DEBUG_PART_INDEX + m_closestPointDistanceThreshold(0) { } @@ -142,9 +153,15 @@ class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result return m_body1Wrap->getCollisionObject(); } + btScalar m_closestPointDistanceThreshold; + /// in the future we can let the user override the methods to combine restitution and friction static btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1); static btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1); + static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1); }; #endif //BT_MANIFOLD_RESULT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp similarity index 90% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 13447822571d..91c76a8dac42 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -199,6 +199,22 @@ class btPersistentManifoldSortPredicate } }; +class btPersistentManifoldSortPredicateDeterministic +{ +public: + + SIMD_FORCE_INLINE bool operator() (const btPersistentManifold* lhs, const btPersistentManifold* rhs) const + { + return ( + (getIslandId(lhs) < getIslandId(rhs)) + || ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) + ||((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) && + (lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId)) + ); + + } +}; + void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld) { @@ -245,13 +261,11 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - if (colObj0->getActivationState()== ACTIVE_TAG) - { - allSleeping = false; - } - if (colObj0->getActivationState()== DISABLE_DEACTIVATION) + if (colObj0->getActivationState()== ACTIVE_TAG || + colObj0->getActivationState()== DISABLE_DEACTIVATION) { allSleeping = false; + break; } } } @@ -318,7 +332,12 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio for (i=0;igetManifoldByIndexInternal(i); - + if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) + { + if (manifold->getNumContacts() == 0) + continue; + } + const btCollisionObject* colObj0 = static_cast(manifold->getBody0()); const btCollisionObject* colObj1 = static_cast(manifold->getBody1()); @@ -379,7 +398,16 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, //tried a radix sort, but quicksort/heapsort seems still faster //@todo rewrite island management - m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + //btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid, + //but also based on object0 unique id and object1 unique id + if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) + { + m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic()); + } else + { + m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + } + //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate()); //now process all active islands (sets of manifolds for now) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp similarity index 97% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index 36ba21f5bb0c..27eaec3059fb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -12,6 +12,7 @@ subject to the following restrictions: 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 CLEAR_MANIFOLD 1 #include "btSphereSphereCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" @@ -62,7 +63,7 @@ void btSphereSphereCollisionAlgorithm::processCollision (const btCollisionObject #endif ///iff distance positive, don't generate a new contact - if ( len > (radius0+radius1)) + if ( len > (radius0+radius1+resultOut->m_closestPointDistanceThreshold)) { #ifndef CLEAR_MANIFOLD resultOut->refreshContactPoints(); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp similarity index 97% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index 280a4d355fdf..86d4e7440094 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/extern/bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -56,7 +56,7 @@ void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObje /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); - SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()); + SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold); btDiscreteCollisionDetectorInterface::ClosestPointInput input; input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet/src/BulletCollision/CollisionDispatch/btUnionFind.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp rename to extern/bullet/src/BulletCollision/CollisionDispatch/btUnionFind.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet/src/BulletCollision/CollisionDispatch/btUnionFind.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h rename to extern/bullet/src/BulletCollision/CollisionDispatch/btUnionFind.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.h similarity index 99% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.h index ce333783e44d..22bee4f2c839 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btBox2dShape.h @@ -103,11 +103,12 @@ ATTRIBUTE_ALIGNED16(class) btBox2dShape: public btPolyhedralConvexShape btScalar minDimension = boxHalfExtents.getX(); if (minDimension>boxHalfExtents.getY()) minDimension = boxHalfExtents.getY(); - setSafeMargin(minDimension); m_shapeType = BOX_2D_SHAPE_PROXYTYPE; btVector3 margin(getMargin(),getMargin(),getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; + + setSafeMargin(minDimension); }; virtual void setMargin(btScalar collisionMargin) diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btBoxShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btBoxShape.cpp index 3859138f18af..72eeb38911f2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btBoxShape.cpp @@ -19,10 +19,10 @@ btBoxShape::btBoxShape( const btVector3& boxHalfExtents) { m_shapeType = BOX_SHAPE_PROXYTYPE; - setSafeMargin(boxHalfExtents); - btVector3 margin(getMargin(),getMargin(),getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; + + setSafeMargin(boxHalfExtents); }; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btBoxShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btBoxShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp similarity index 98% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index ace4cfa26465..61f465cb729e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -245,16 +245,18 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co btStridingMeshInterface* m_meshInterface; btTriangleCallback* m_callback; btVector3 m_triangle[3]; - + int m_numOverlap; MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) :m_meshInterface(meshInterface), - m_callback(callback) + m_callback(callback), + m_numOverlap(0) { } virtual void processNode(int nodeSubPart, int nodeTriangleIndex) { + m_numOverlap++; const unsigned char *vertexbase; int numverts; PHY_ScalarType type; @@ -321,8 +323,7 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); - - + #endif//DISABLE_BVH @@ -436,6 +437,9 @@ const char* btBvhTriangleMeshShape::serialize(void* dataBuffer, btSerializer* se trimeshData->m_triangleInfoMap = 0; } + // Fill padding with zeros to appease msan. + memset(trimeshData->m_pad3, 0, sizeof(trimeshData->m_pad3)); + return "btTriangleMeshShapeData"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp similarity index 79% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp index 94709dca599c..0345501ce24f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -16,16 +16,14 @@ subject to the following restrictions: #include "btCapsuleShape.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "LinearMath/btQuaternion.h" btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape () { + m_collisionMargin = radius; m_shapeType = CAPSULE_SHAPE_PROXYTYPE; m_upAxis = 1; m_implicitShapeDimensions.setValue(radius,0.5f*height,radius); - btVector3 margin(getMargin(),getMargin(),getMargin()); - m_implicitShapeDimensions -= margin; } @@ -50,14 +48,13 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter btVector3 vtx; btScalar newDot; - btScalar radius = getRadius(); - + { btVector3 pos(0,0,0); - pos[getUpAxis()] = getExtend(); + pos[getUpAxis()] = getHalfHeight(); - vtx = pos +vec*(radius); + vtx = pos; newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -67,9 +64,9 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter } { btVector3 pos(0,0,0); - pos[getUpAxis()] = -getExtend(); + pos[getUpAxis()] = -getHalfHeight(); - vtx = pos +vec*(radius); + vtx = pos; newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -86,8 +83,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter { - btScalar radius = getRadius(); - + for (int j=0;j maxDot) { @@ -108,8 +104,8 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter } { btVector3 pos(0,0,0); - pos[getUpAxis()] = -getExtend(); - vtx = pos +vec*(radius); + pos[getUpAxis()] = -getHalfHeight(); + vtx = pos; newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -133,13 +129,11 @@ void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) con btScalar radius = getRadius(); btVector3 halfExtents(radius,radius,radius); - halfExtents[getUpAxis()]+=getExtend(); - - btScalar margin = getMargin(); + halfExtents[getUpAxis()]+=getHalfHeight(); - btScalar lx=btScalar(2.)*(halfExtents[0]+margin); - btScalar ly=btScalar(2.)*(halfExtents[1]+margin); - btScalar lz=btScalar(2.)*(halfExtents[2]+margin); + btScalar lx=btScalar(2.)*(halfExtents[0]); + btScalar ly=btScalar(2.)*(halfExtents[1]); + btScalar lz=btScalar(2.)*(halfExtents[2]); const btScalar x2 = lx*lx; const btScalar y2 = ly*ly; const btScalar z2 = lz*lz; @@ -153,10 +147,9 @@ void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) con btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height) { + m_collisionMargin = radius; m_upAxis = 0; m_implicitShapeDimensions.setValue(0.5f*height, radius,radius); - btVector3 margin(getMargin(),getMargin(),getMargin()); - m_implicitShapeDimensions -= margin; } @@ -166,10 +159,9 @@ btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height) btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height) { + m_collisionMargin = radius; m_upAxis = 2; m_implicitShapeDimensions.setValue(radius,radius,0.5f*height); - btVector3 margin(getMargin(),getMargin(),getMargin()); - m_implicitShapeDimensions -= margin; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btCapsuleShape.h similarity index 83% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btCapsuleShape.h index 0cd256f95da1..5a3362834a5a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -48,20 +48,14 @@ ATTRIBUTE_ALIGNED16(class) btCapsuleShape : public btConvexInternalShape virtual void setMargin(btScalar collisionMargin) { - //correct the m_implicitShapeDimensions for the margin - btVector3 oldMargin(getMargin(),getMargin(),getMargin()); - btVector3 implicitShapeDimensionsWithMargin = m_implicitShapeDimensions+oldMargin; - - btConvexInternalShape::setMargin(collisionMargin); - btVector3 newMargin(getMargin(),getMargin(),getMargin()); - m_implicitShapeDimensions = implicitShapeDimensionsWithMargin - newMargin; + //don't override the margin for capsules, their entire radius == margin + (void)collisionMargin; } virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const { btVector3 halfExtents(getRadius(),getRadius(),getRadius()); - halfExtents[m_upAxis] += getExtend(); - halfExtents += btVector3(getMargin(),getMargin(),getMargin()); + halfExtents[m_upAxis] = getRadius() + getHalfHeight(); btMatrix3x3 abs_b = t.getBasis().absolute(); btVector3 center = t.getOrigin(); btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); @@ -91,27 +85,14 @@ ATTRIBUTE_ALIGNED16(class) btCapsuleShape : public btConvexInternalShape return m_implicitShapeDimensions[m_upAxis]; } - btScalar getExtend() const - { - return getHalfHeight() - getRadius(); - } - virtual void setLocalScaling(const btVector3& scaling) { - const unsigned short r1 = (m_upAxis + 1) % 3; - const unsigned short r2 = (m_upAxis + 2) % 3; - - const float margin = getMargin(); - const float halfHeight = m_implicitShapeDimensions[m_upAxis]; - float radius = m_implicitShapeDimensions[r2]; - float radiusWithMargin = radius + margin; - const float halfHeightWithMargin = halfHeight + margin; - - const btVector3 scaleRatio = scaling / m_localScaling; - m_implicitShapeDimensions[r2] = radiusWithMargin * (scaleRatio[r1] + scaleRatio[r2]) / 2.0f - margin; - - m_implicitShapeDimensions[m_upAxis] = halfHeightWithMargin * scaleRatio[m_upAxis] - margin; - btConvexInternalShape::setLocalScaling(scaling); + btVector3 unScaledImplicitShapeDimensions = m_implicitShapeDimensions / m_localScaling; + btConvexInternalShape::setLocalScaling(scaling); + m_implicitShapeDimensions = (unScaledImplicitShapeDimensions * scaling); + //update m_collisionMargin, since entire radius==margin + int radiusAxis = (m_upAxis+2)%3; + m_collisionMargin = m_implicitShapeDimensions[radiusAxis]; } virtual btVector3 getAnisotropicRollingFrictionDirection() const @@ -184,11 +165,17 @@ SIMD_FORCE_INLINE int btCapsuleShape::calculateSerializeBufferSize() const SIMD_FORCE_INLINE const char* btCapsuleShape::serialize(void* dataBuffer, btSerializer* serializer) const { btCapsuleShapeData* shapeData = (btCapsuleShapeData*) dataBuffer; - + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer); shapeData->m_upAxis = m_upAxis; - + + // Fill padding with zeros to appease msan. + shapeData->m_padding[0] = 0; + shapeData->m_padding[1] = 0; + shapeData->m_padding[2] = 0; + shapeData->m_padding[3] = 0; + return "btCapsuleShapeData"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h b/extern/bullet/src/BulletCollision/CollisionShapes/btCollisionMargin.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btCollisionMargin.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.cpp similarity index 97% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.cpp index 39ee21cad73c..823e2788f2e7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.cpp @@ -106,7 +106,10 @@ const char* btCollisionShape::serialize(void* dataBuffer, btSerializer* serializ serializer->serializeName(name); } shapeData->m_shapeType = m_shapeType; - //shapeData->m_padding//?? + + // Fill padding with zeros to appease msan. + memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding)); + return "btCollisionShapeData"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.h similarity index 98% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.h index 5e86568005ec..6c4916fbd4bd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -48,7 +48,7 @@ ATTRIBUTE_ALIGNED16(class) btCollisionShape virtual void getBoundingSphere(btVector3& center,btScalar& radius) const; - ///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations. + ///getAngularMotionDisc returns the maximum radius needed for Conservative Advancement to handle time-of-impact with rotations. virtual btScalar getAngularMotionDisc() const; virtual btScalar getContactBreakingThreshold(btScalar defaultContactThresholdFactor) const; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.cpp similarity index 98% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.cpp index e8c8c336cd2c..85572da307db 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -213,7 +213,7 @@ void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co -void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const +void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const { int n = m_children.size(); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.h similarity index 98% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.h index 4eef8dba3068..2cbcd1bfca26 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -160,7 +160,7 @@ ATTRIBUTE_ALIGNED16(class) btCompoundShape : public btCollisionShape ///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform ///of the collision object by the principal transform. - void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const; + void calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const; int getUpdateRevision() const { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btConcaveShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btConcaveShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btConcaveShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btConcaveShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btConeShape.cpp similarity index 99% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btConeShape.cpp index 5e2aaebfe529..2d83c8bfbac5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btConeShape.cpp @@ -144,4 +144,4 @@ void btConeShape::setLocalScaling(const btVector3& scaling) m_radius *= (scaling[r1] / m_localScaling[r1] + scaling[r2] / m_localScaling[r2]) / 2; m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height)); btConvexInternalShape::setLocalScaling(scaling); -} +} \ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btConeShape.h similarity index 94% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btConeShape.h index 4a0df0d558bb..3b44e3f272c4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btConeShape.h @@ -43,6 +43,15 @@ ATTRIBUTE_ALIGNED16(class) btConeShape : public btConvexInternalShape btScalar getRadius() const { return m_radius;} btScalar getHeight() const { return m_height;} + void setRadius(const btScalar radius) + { + m_radius = radius; + } + void setHeight(const btScalar height) + { + m_height = height; + } + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const { @@ -159,11 +168,17 @@ SIMD_FORCE_INLINE int btConeShape::calculateSerializeBufferSize() const SIMD_FORCE_INLINE const char* btConeShape::serialize(void* dataBuffer, btSerializer* serializer) const { btConeShapeData* shapeData = (btConeShapeData*) dataBuffer; - + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer); - + shapeData->m_upIndex = m_coneIndices[1]; - + + // Fill padding with zeros to appease msan. + shapeData->m_padding[0] = 0; + shapeData->m_padding[1] = 0; + shapeData->m_padding[2] = 0; + shapeData->m_padding[3] = 0; + return "btConeShapeData"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btConvex2dShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConvex2dShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btConvex2dShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp similarity index 93% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 02ea5033b156..eec2b8d769ea 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -22,6 +22,8 @@ subject to the following restrictions: #include "LinearMath/btQuaternion.h" #include "LinearMath/btSerializer.h" +#include "btConvexPolyhedron.h" +#include "LinearMath/btConvexHullComputer.h" btConvexHullShape ::btConvexHullShape (const btScalar* points,int numPoints,int stride) : btPolyhedralConvexAabbCachingShape () { @@ -121,10 +123,17 @@ btVector3 btConvexHullShape::localGetSupportingVertex(const btVector3& vec)const } - - - - +void btConvexHullShape::optimizeConvexHull() +{ + btConvexHullComputer conv; + conv.compute(&m_unscaledPoints[0].getX(), sizeof(btVector3),m_unscaledPoints.size(),0.f,0.f); + int numVerts = conv.vertices.size(); + m_unscaledPoints.resize(0); + for (int i=0;ifinalizeChunk(chunk,btVector3DataName,BT_ARRAY_CODE,(void*)&m_unscaledPoints[0]); } - + + // Fill padding with zeros to appease msan. + memset(shapeData->m_padding3, 0, sizeof(shapeData->m_padding3)); + return "btConvexHullShapeData"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.h similarity index 99% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.h index 3bd598ec4e9d..0c12aeef156d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -55,9 +55,8 @@ ATTRIBUTE_ALIGNED16(class) btConvexHullShape : public btPolyhedralConvexAabbCach return getUnscaledPoints(); } - - - + void optimizeConvexHull(); + SIMD_FORCE_INLINE btVector3 getScaledPoint(int i) const { return m_unscaledPoints[i] * m_localScaling; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.h similarity index 98% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.h index 37e04f5fc81b..1213b82fbe31 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexInternalShape.h @@ -172,6 +172,9 @@ SIMD_FORCE_INLINE const char* btConvexInternalShape::serialize(void* dataBuffer, m_localScaling.serializeFloat(shapeData->m_localScaling); shapeData->m_collisionMargin = float(m_collisionMargin); + // Fill padding with zeros to appease msan. + shapeData->m_padding = 0; + return "btConvexInternalShapeData"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp similarity index 99% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index 4f45319a83a9..0fea00df5c16 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -104,9 +104,9 @@ void btConvexPolyhedron::initialize() btHashMap edges; - btScalar TotalArea = 0.0f; - m_localCenter.setValue(0, 0, 0); + + for(int i=0;i= 0); + if (ptIndex<0) + { + ptIndex = 0; + } btVector3 supVec = points[ptIndex] * localScaling; return supVec; #endif //__SPU__ @@ -227,17 +231,16 @@ btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btV btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); btCapsuleShape* capsuleShape = (btCapsuleShape*)this; - btScalar extend = capsuleShape->getExtend(); + 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)) + if (lenSqr < SIMD_EPSILON*SIMD_EPSILON) { vec.setValue(1,0,0); } else @@ -249,9 +252,9 @@ btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btV btScalar newDot; { btVector3 pos(0,0,0); - pos[capsuleUpAxis] = extend; + pos[capsuleUpAxis] = halfHeight; - vtx = pos +vec*(radius); + vtx = pos; newDot = vec.dot(vtx); @@ -263,9 +266,9 @@ btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btV } { btVector3 pos(0,0,0); - pos[capsuleUpAxis] = -extend; + pos[capsuleUpAxis] = -halfHeight; - vtx = pos +vec*(radius); + vtx = pos; newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -424,8 +427,7 @@ void btConvexShape::getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btCapsuleShape* capsuleShape = (btCapsuleShape*)this; btVector3 halfExtents(capsuleShape->getRadius(),capsuleShape->getRadius(),capsuleShape->getRadius()); int m_upAxis = capsuleShape->getUpAxis(); - halfExtents[m_upAxis] += capsuleShape->getExtend(); - halfExtents += btVector3(capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual(),capsuleShape->getMarginNonVirtual()); + halfExtents[m_upAxis] = capsuleShape->getRadius() + capsuleShape->getHalfHeight(); btMatrix3x3 abs_b = t.getBasis().absolute(); btVector3 center = t.getOrigin(); btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btConvexShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btCylinderShape.cpp similarity index 99% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btCylinderShape.cpp index 6cfe43be4da7..604b3fc77085 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -19,10 +19,11 @@ btCylinderShape::btCylinderShape (const btVector3& halfExtents) :btConvexInternalShape(), m_upAxis(1) { - setSafeMargin(halfExtents); - btVector3 margin(getMargin(),getMargin(),getMargin()); m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin; + + setSafeMargin(halfExtents); + m_shapeType = CYLINDER_SHAPE_PROXYTYPE; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btCylinderShape.h similarity index 97% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btCylinderShape.h index 6f796950e1dd..a214a827c9d0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btCylinderShape.h @@ -199,11 +199,17 @@ SIMD_FORCE_INLINE int btCylinderShape::calculateSerializeBufferSize() const SIMD_FORCE_INLINE const char* btCylinderShape::serialize(void* dataBuffer, btSerializer* serializer) const { btCylinderShapeData* shapeData = (btCylinderShapeData*) dataBuffer; - + btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData,serializer); shapeData->m_upAxis = m_upAxis; - + + // Fill padding with zeros to appease msan. + shapeData->m_padding[0] = 0; + shapeData->m_padding[1] = 0; + shapeData->m_padding[2] = 0; + shapeData->m_padding[3] = 0; + return "btCylinderShapeData"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btEmptyShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btEmptyShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btEmptyShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btEmptyShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h b/extern/bullet/src/BulletCollision/CollisionShapes/btMaterial.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btMaterial.h diff --git a/extern/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.cpp new file mode 100644 index 000000000000..afe45e1d2d02 --- /dev/null +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.cpp @@ -0,0 +1,532 @@ +#include "btMiniSDF.h" + +// +//Based on code from DiscreGrid, https://github.com/InteractiveComputerGraphics/Discregrid +//example: +//GenerateSDF.exe -r "32 32 32" -d "-1.6 -1.6 -.6 1.6 1.6 .6" concave_box.obj +//The MIT License (MIT) +// +//Copyright (c) 2017 Dan Koschier +// + +#include +#include //memcpy + +struct btSdfDataStream +{ + const char* m_data; + int m_size; + + int m_currentOffset; + + btSdfDataStream(const char* data, int size) + :m_data(data), + m_size(size), + m_currentOffset(0) + { + + } + + template bool read(T& val) + { + int bytes = sizeof(T); + if (m_currentOffset+bytes<=m_size) + { + char* dest = (char*)&val; + memcpy(dest,&m_data[m_currentOffset],bytes); + m_currentOffset+=bytes; + return true; + } + btAssert(0); + return false; + } +}; + + +bool btMiniSDF::load(const char* data, int size) +{ + int fileSize = -1; + + btSdfDataStream ds(data,size); + { + double buf[6]; + ds.read(buf); + m_domain.m_min[0] = buf[0]; + m_domain.m_min[1] = buf[1]; + m_domain.m_min[2] = buf[2]; + m_domain.m_min[3] = 0; + m_domain.m_max[0] = buf[3]; + m_domain.m_max[1] = buf[4]; + m_domain.m_max[2] = buf[5]; + m_domain.m_max[3] = 0; + } + { + unsigned int buf2[3]; + ds.read(buf2); + m_resolution[0] = buf2[0]; + m_resolution[1] = buf2[1]; + m_resolution[2] = buf2[2]; + } + { + double buf[3]; + ds.read(buf); + m_cell_size[0] = buf[0]; + m_cell_size[1] = buf[1]; + m_cell_size[2] = buf[2]; + } + { + double buf[3]; + ds.read(buf); + m_inv_cell_size[0] = buf[0]; + m_inv_cell_size[1] = buf[1]; + m_inv_cell_size[2] = buf[2]; + } + { + unsigned long long int cells; + ds.read(cells); + m_n_cells = cells; + } + { + unsigned long long int fields; + ds.read(fields); + m_n_fields = fields; + } + + + unsigned long long int nodes0; + std::size_t n_nodes0; + ds.read(nodes0); + n_nodes0 = nodes0; + if (n_nodes0 > 1024 * 1024 * 1024) + { + return m_isValid; + } + m_nodes.resize(n_nodes0); + for (unsigned int i=0;i& nodes = m_nodes[i]; + nodes.resize(n_nodes1); + for ( int j=0;j& cells = m_cells[i]; + ds.read(n_cells1); + cells.resize(n_cells1); + for (int j=0;j& cell_maps = m_cell_map[i]; + ds.read(n_cell_maps1); + cell_maps.resize(n_cell_maps1); + for (int j=0;j().eval(); + unsigned int mi[3] = {(unsigned int )tmpmi[0],(unsigned int )tmpmi[1],(unsigned int )tmpmi[2]}; + if (mi[0] >= m_resolution[0]) + mi[0] = m_resolution[0]-1; + if (mi[1] >= m_resolution[1]) + mi[1] = m_resolution[1]-1; + if (mi[2] >= m_resolution[2]) + mi[2] = m_resolution[2]-1; + btMultiIndex mui; + mui.ijk[0] = mi[0]; + mui.ijk[1] = mi[1]; + mui.ijk[2] = mi[2]; + int i = multiToSingleIndex(mui); + unsigned int i_ = m_cell_map[field_id][i]; + if (i_ == UINT_MAX) + return false; + + btAlignedBox3d sd = subdomain(i); + i = i_; + btVector3 d = sd.m_max-sd.m_min;//.diagonal().eval(); + + btVector3 denom = (sd.max() - sd.min()); + btVector3 c0 = btVector3(2.0,2.0,2.0)/denom; + btVector3 c1 = (sd.max() + sd.min())/denom; + btVector3 xi = (c0*x - c1); + + btCell32 const& cell = m_cells[field_id][i]; + if (!gradient) + { + //auto phi = m_coefficients[field_id][i].dot(shape_function_(xi, 0)); + double phi = 0.0; + btShapeMatrix N = shape_function_(xi, 0); + for (unsigned int j = 0u; j < 32u; ++j) + { + unsigned int v = cell.m_cells[j]; + double c = m_nodes[field_id][v]; + if (c == DBL_MAX) + { + return false;; + } + phi += c * N[j]; + } + + dist = phi; + return true; + } + + btShapeGradients dN; + btShapeMatrix N = shape_function_(xi, &dN); + + double phi = 0.0; + gradient->setZero(); + for (unsigned int j = 0u; j < 32u; ++j) + { + unsigned int v = cell.m_cells[j]; + double c = m_nodes[field_id][v]; + if (c == DBL_MAX) + { + gradient->setZero(); + return false; + } + phi += c * N[j]; + (*gradient)[0] += c * dN(j, 0); + (*gradient)[1] += c * dN(j, 1); + (*gradient)[2] += c * dN(j, 2); + } + (*gradient) *= c0; + dist = phi; + return true; +} + diff --git a/extern/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.h b/extern/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.h new file mode 100644 index 000000000000..3de90e4f8ad5 --- /dev/null +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btMiniSDF.h @@ -0,0 +1,134 @@ +#ifndef MINISDF_H +#define MINISDF_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAabbUtil2.h" +#include "LinearMath/btAlignedObjectArray.h" + + +struct btMultiIndex +{ + unsigned int ijk[3]; +}; + +struct btAlignedBox3d +{ + btVector3 m_min; + btVector3 m_max; + + const btVector3& min() const + { + return m_min; + } + + const btVector3& max() const + { + return m_max; + } + + + bool contains(const btVector3& x) const + { + return TestPointAgainstAabb2(m_min, m_max, x); + } + + btAlignedBox3d(const btVector3& mn, const btVector3& mx) + :m_min(mn), + m_max(mx) + { + } + + btAlignedBox3d() + { + } +}; + +struct btShapeMatrix +{ + double m_vec[32]; + + inline double& operator[](int i) + { + return m_vec[i]; + } + + inline const double& operator[](int i) const + { + return m_vec[i]; + } + +}; + +struct btShapeGradients +{ + btVector3 m_vec[32]; + + void topRowsDivide(int row, double denom) + { + for (int i=0;i > m_nodes; + btAlignedObjectArray > m_cells; + btAlignedObjectArray > m_cell_map; + + btMiniSDF() + :m_isValid(false) + { + } + bool load(const char* data, int size); + bool isValid() const + { + return m_isValid; + } + unsigned int multiToSingleIndex(btMultiIndex const & ijk) const; + + btAlignedBox3d subdomain(btMultiIndex const& ijk) const; + + btMultiIndex singleToMultiIndex(unsigned int l) const; + + btAlignedBox3d subdomain(unsigned int l) const; + + + btShapeMatrix + shape_function_(btVector3 const& xi, btShapeGradients* gradient = 0) const; + + bool interpolate(unsigned int field_id, double& dist, btVector3 const& x, btVector3* gradient) const; +}; + + +#endif //MINISDF_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp similarity index 78% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp index 06707e24e552..899ef5005647 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -55,6 +55,23 @@ btScalar btMinkowskiSumShape::getMargin() const void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { (void)mass; - btAssert(0); - inertia.setValue(0,0,0); + //inertia of the AABB of the Minkowski sum + btTransform identity; + identity.setIdentity(); + btVector3 aabbMin,aabbMax; + getAabb(identity,aabbMin,aabbMax); + + btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); + + btScalar margin = getMargin(); + + 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)); } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp similarity index 94% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp index 6abfdffa6775..d5bf6d60fe3f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -75,7 +75,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala int inner_count = MIN( numSpheres - k, 128 ); for( long i = 0; i < inner_count; i++ ) { - temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin(); + temp[i] = (*pos)*m_localScaling +vec*m_localScaling*(*rad) - vec * getMargin(); pos++; rad++; } @@ -113,7 +113,7 @@ btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScala int inner_count = MIN( numSpheres - k, 128 ); for( long i = 0; i < inner_count; i++ ) { - temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin(); + temp[i] = (*pos)*m_localScaling +vec*m_localScaling*(*rad) - vec * getMargin(); pos++; rad++; } @@ -175,7 +175,10 @@ const char* btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serial } serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]); } - + + // Fill padding with zeros to appease msan. + memset(shapeData->m_padding, 0, sizeof(shapeData->m_padding)); + return "btMultiSphereShapeData"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btMultiSphereShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/extern/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btOptimizedBvh.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp similarity index 86% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 9095c592d87d..0246c2d59195 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -39,6 +39,17 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape() } } +void btPolyhedralConvexShape::setPolyhedralFeatures(btConvexPolyhedron& polyhedron) +{ + if (m_polyhedron) + { + *m_polyhedron = polyhedron; + } else + { + void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16); + m_polyhedron = new (mem) btConvexPolyhedron(polyhedron); + } +} bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin) { @@ -87,8 +98,72 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); } +#ifndef BT_RECONSTRUCT_FACES + + int numVertices = conv.vertices.size(); + m_polyhedron->m_vertices.resize(numVertices); + for (int p=0;pm_vertices[p] = conv.vertices[p]; + } + + int v0, v1; + for (int j = 0; j < conv.faces.size(); j++) + { + btVector3 edges[3]; + int numEdges = 0; + btFace combinedFace; + const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]]; + v0 = edge->getSourceVertex(); + int prevVertex=v0; + combinedFace.m_indices.push_back(v0); + v1 = edge->getTargetVertex(); + while (v1 != v0) + { + + btVector3 wa = conv.vertices[prevVertex]; + btVector3 wb = conv.vertices[v1]; + btVector3 newEdge = wb-wa; + newEdge.normalize(); + if (numEdges<2) + edges[numEdges++] = newEdge; + //face->addIndex(v1); + combinedFace.m_indices.push_back(v1); + edge = edge->getNextEdgeOfFace(); + prevVertex = v1; + int v01 = edge->getSourceVertex(); + v1 = edge->getTargetVertex(); + + } + + btAssert(combinedFace.m_indices.size() > 2); + + btVector3 faceNormal = edges[0].cross(edges[1]); + faceNormal.normalize(); + + btScalar planeEq=1e30f; + + for (int v=0;vm_vertices[combinedFace.m_indices[v]].dot(faceNormal); + if (planeEq>eq) + { + planeEq=eq; + } + } + combinedFace.m_plane[0] = faceNormal.getX(); + combinedFace.m_plane[1] = faceNormal.getY(); + combinedFace.m_plane[2] = faceNormal.getZ(); + combinedFace.m_plane[3] = -planeEq; + + m_polyhedron->m_faces.push_back(combinedFace); + } + + +#else//BT_RECONSTRUCT_FACES + btAlignedObjectArray faceNormals; int numFaces = conv.faces.size(); faceNormals.resize(numFaces); @@ -311,7 +386,9 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa } - + +#endif //BT_RECONSTRUCT_FACES + m_polyhedron->initialize(); return true; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h similarity index 97% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index 961d001a9dd2..b7ddb6e06080 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -43,6 +43,9 @@ ATTRIBUTE_ALIGNED16(class) btPolyhedralConvexShape : public btConvexInternalShap ///experimental/work-in-progress virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin=0); + virtual void setPolyhedralFeatures(btConvexPolyhedron& polyhedron); + + const btConvexPolyhedron* getConvexPolyhedron() const { return m_polyhedron; @@ -93,10 +96,12 @@ class btPolyhedralConvexAabbCachingShape : public btPolyhedralConvexShape aabbMax = m_localAabbMax; } -public: +protected: btPolyhedralConvexAabbCachingShape(); +public: + inline void getNonvirtualAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax, btScalar margin) const { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h diff --git a/extern/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp new file mode 100644 index 000000000000..828acda47094 --- /dev/null +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp @@ -0,0 +1,99 @@ +#include "btSdfCollisionShape.h" +#include "btMiniSDF.h" +#include "LinearMath/btAabbUtil2.h" + +struct btSdfCollisionShapeInternalData +{ + btVector3 m_localScaling; + btScalar m_margin; + btMiniSDF m_sdf; + + btSdfCollisionShapeInternalData() + :m_localScaling(1,1,1), + m_margin(0) + { + + } +}; + +bool btSdfCollisionShape::initializeSDF(const char* sdfData, int sizeInBytes) +{ + bool valid = m_data->m_sdf.load(sdfData, sizeInBytes); + return valid; +} +btSdfCollisionShape::btSdfCollisionShape() +{ + m_shapeType = SDF_SHAPE_PROXYTYPE; + m_data = new btSdfCollisionShapeInternalData(); + + + + //"E:/develop/bullet3/data/toys/ground_hole64_64_8.cdf");//ground_cube.cdf"); + /*unsigned int field_id=0; + Eigen::Vector3d x (1,10,1); + Eigen::Vector3d gradient; + double dist = m_data->m_sdf.interpolate(field_id, x, &gradient); + printf("dist=%g\n", dist); + */ + +} +btSdfCollisionShape::~btSdfCollisionShape() +{ + delete m_data; +} + +void btSdfCollisionShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + btAssert(m_data->m_sdf.isValid()); + btVector3 localAabbMin = m_data->m_sdf.m_domain.m_min; + btVector3 localAabbMax = m_data->m_sdf.m_domain.m_max; + btScalar margin(0); + btTransformAabb(localAabbMin,localAabbMax,margin,t,aabbMin,aabbMax); + +} + + +void btSdfCollisionShape::setLocalScaling(const btVector3& scaling) +{ + m_data->m_localScaling = scaling; +} +const btVector3& btSdfCollisionShape::getLocalScaling() const +{ + return m_data->m_localScaling; +} +void btSdfCollisionShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + inertia.setValue(0,0,0); +} +const char* btSdfCollisionShape::getName()const +{ + return "btSdfCollisionShape"; +} +void btSdfCollisionShape::setMargin(btScalar margin) +{ + m_data->m_margin = margin; +} +btScalar btSdfCollisionShape::getMargin() const +{ + return m_data->m_margin; +} + +void btSdfCollisionShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + //not yet +} + + +bool btSdfCollisionShape::queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal) +{ + int field = 0; + btVector3 grad; + double dist; + bool hasResult = m_data->m_sdf.interpolate(field,dist, ptInSDF,&grad); + if (hasResult) + { + normal.setValue(grad[0],grad[1],grad[2]); + distOut= dist; + } + return hasResult; +} diff --git a/extern/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h new file mode 100644 index 000000000000..6e32db9cd861 --- /dev/null +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btSdfCollisionShape.h @@ -0,0 +1,30 @@ +#ifndef BT_SDF_COLLISION_SHAPE_H +#define BT_SDF_COLLISION_SHAPE_H + +#include "btConcaveShape.h" + +class btSdfCollisionShape : public btConcaveShape +{ + struct btSdfCollisionShapeInternalData* m_data; + +public: + + btSdfCollisionShape(); + virtual ~btSdfCollisionShape(); + + bool initializeSDF(const char* sdfData, int sizeInBytes); + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual const char* getName()const; + virtual void setMargin(btScalar margin); + virtual btScalar getMargin() const; + + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + bool queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal); +}; + +#endif //BT_SDF_COLLISION_SHAPE_H diff --git a/extern/bullet/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btShapeHull.cpp new file mode 100644 index 000000000000..9f712fe555ab --- /dev/null +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -0,0 +1,433 @@ +/* +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. +*/ + +//btShapeHull was implemented by John McCutchan. + + +#include "btShapeHull.h" +#include "LinearMath/btConvexHull.h" + +#define NUM_UNITSPHERE_POINTS 42 +#define NUM_UNITSPHERE_POINTS_HIGHRES 256 + + +btShapeHull::btShapeHull (const btConvexShape* shape) +{ + m_shape = shape; + m_vertices.clear (); + m_indices.clear(); + m_numIndices = 0; +} + +btShapeHull::~btShapeHull () +{ + m_indices.clear(); + m_vertices.clear (); +} + +bool +btShapeHull::buildHull (btScalar /*margin*/, int highres) +{ + int numSampleDirections = highres? NUM_UNITSPHERE_POINTS_HIGHRES:NUM_UNITSPHERE_POINTS; + { + int numPDA = m_shape->getNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int i=0;igetPreferredPenetrationDirection(i,norm); + getUnitSpherePoints(highres)[numSampleDirections] = norm; + numSampleDirections++; + } + } + } + + btVector3 supportPoints[NUM_UNITSPHERE_POINTS_HIGHRES+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + int i; + for (i = 0; i < numSampleDirections; i++) + { + supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints(highres)[i]); + } + + HullDesc hd; + hd.mFlags = QF_TRIANGLES; + hd.mVcount = static_cast(numSampleDirections); + +#ifdef BT_USE_DOUBLE_PRECISION + hd.mVertices = &supportPoints[0]; + hd.mVertexStride = sizeof(btVector3); +#else + hd.mVertices = &supportPoints[0]; + hd.mVertexStride = sizeof (btVector3); +#endif + + HullLibrary hl; + HullResult hr; + if (hl.CreateConvexHull (hd, hr) == QE_FAIL) + { + return false; + } + + m_vertices.resize (static_cast(hr.mNumOutputVertices)); + + + for (i = 0; i < static_cast(hr.mNumOutputVertices); i++) + { + m_vertices[i] = hr.m_OutputVertices[i]; + } + m_numIndices = hr.mNumIndices; + m_indices.resize(static_cast(m_numIndices)); + for (i = 0; i < static_cast(m_numIndices); i++) + { + m_indices[i] = hr.m_Indices[i]; + } + + // free temporary hull result that we just copied + hl.ReleaseResult (hr); + + return true; +} + +int +btShapeHull::numTriangles () const +{ + return static_cast(m_numIndices / 3); +} + +int +btShapeHull::numVertices () const +{ + return m_vertices.size (); +} + +int +btShapeHull::numIndices () const +{ + return static_cast(m_numIndices); +} + + +btVector3* btShapeHull::getUnitSpherePoints(int highres) +{ + static btVector3 sUnitSpherePointsHighres[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] = + { + btVector3(btScalar(0.997604), btScalar(0.067004), btScalar(0.017144)), + btVector3(btScalar(0.984139), btScalar(-0.086784), btScalar(-0.154427)), + btVector3(btScalar(0.971065), btScalar(0.124164), btScalar(-0.203224)), + btVector3(btScalar(0.955844), btScalar(0.291173), btScalar(-0.037704)), + btVector3(btScalar(0.957405), btScalar(0.212238), btScalar(0.195157)), + btVector3(btScalar(0.971650), btScalar(-0.012709), btScalar(0.235561)), + btVector3(btScalar(0.984920), btScalar(-0.161831), btScalar(0.059695)), + btVector3(btScalar(0.946673), btScalar(-0.299288), btScalar(-0.117536)), + btVector3(btScalar(0.922670), btScalar(-0.219186), btScalar(-0.317019)), + btVector3(btScalar(0.928134), btScalar(-0.007265), btScalar(-0.371867)), + btVector3(btScalar(0.875642), btScalar(0.198434), btScalar(-0.439988)), + btVector3(btScalar(0.908035), btScalar(0.325975), btScalar(-0.262562)), + btVector3(btScalar(0.864519), btScalar(0.488706), btScalar(-0.116755)), + btVector3(btScalar(0.893009), btScalar(0.428046), btScalar(0.137185)), + btVector3(btScalar(0.857494), btScalar(0.362137), btScalar(0.364776)), + btVector3(btScalar(0.900815), btScalar(0.132524), btScalar(0.412987)), + btVector3(btScalar(0.934964), btScalar(-0.241739), btScalar(0.259179)), + btVector3(btScalar(0.894570), btScalar(-0.103504), btScalar(0.434263)), + btVector3(btScalar(0.922085), btScalar(-0.376668), btScalar(0.086241)), + btVector3(btScalar(0.862177), btScalar(-0.499154), btScalar(-0.085330)), + btVector3(btScalar(0.861982), btScalar(-0.420218), btScalar(-0.282861)), + btVector3(btScalar(0.818076), btScalar(-0.328256), btScalar(-0.471804)), + btVector3(btScalar(0.762657), btScalar(-0.179329), btScalar(-0.621124)), + btVector3(btScalar(0.826857), btScalar(0.019760), btScalar(-0.561786)), + btVector3(btScalar(0.731434), btScalar(0.206599), btScalar(-0.649817)), + btVector3(btScalar(0.769486), btScalar(0.379052), btScalar(-0.513770)), + btVector3(btScalar(0.796806), btScalar(0.507176), btScalar(-0.328145)), + btVector3(btScalar(0.679722), btScalar(0.684101), btScalar(-0.264123)), + btVector3(btScalar(0.786854), btScalar(0.614886), btScalar(0.050912)), + btVector3(btScalar(0.769486), btScalar(0.571141), btScalar(0.285139)), + btVector3(btScalar(0.707432), btScalar(0.492789), btScalar(0.506288)), + btVector3(btScalar(0.774560), btScalar(0.268037), btScalar(0.572652)), + btVector3(btScalar(0.796220), btScalar(0.031230), btScalar(0.604077)), + btVector3(btScalar(0.837395), btScalar(-0.320285), btScalar(0.442461)), + btVector3(btScalar(0.848127), btScalar(-0.450548), btScalar(0.278307)), + btVector3(btScalar(0.775536), btScalar(-0.206354), btScalar(0.596465)), + btVector3(btScalar(0.816320), btScalar(-0.567007), btScalar(0.109469)), + btVector3(btScalar(0.741191), btScalar(-0.668690), btScalar(-0.056832)), + btVector3(btScalar(0.755632), btScalar(-0.602975), btScalar(-0.254949)), + btVector3(btScalar(0.720311), btScalar(-0.521318), btScalar(-0.457165)), + btVector3(btScalar(0.670746), btScalar(-0.386583), btScalar(-0.632835)), + btVector3(btScalar(0.587031), btScalar(-0.219769), btScalar(-0.778836)), + btVector3(btScalar(0.676015), btScalar(-0.003182), btScalar(-0.736676)), + btVector3(btScalar(0.566932), btScalar(0.186963), btScalar(-0.802064)), + btVector3(btScalar(0.618254), btScalar(0.398105), btScalar(-0.677533)), + btVector3(btScalar(0.653964), btScalar(0.575224), btScalar(-0.490933)), + btVector3(btScalar(0.525367), btScalar(0.743205), btScalar(-0.414028)), + btVector3(btScalar(0.506439), btScalar(0.836528), btScalar(-0.208885)), + btVector3(btScalar(0.651427), btScalar(0.756426), btScalar(-0.056247)), + btVector3(btScalar(0.641670), btScalar(0.745149), btScalar(0.180908)), + btVector3(btScalar(0.602643), btScalar(0.687211), btScalar(0.405180)), + btVector3(btScalar(0.516586), btScalar(0.596999), btScalar(0.613447)), + btVector3(btScalar(0.602252), btScalar(0.387801), btScalar(0.697573)), + btVector3(btScalar(0.646549), btScalar(0.153911), btScalar(0.746956)), + btVector3(btScalar(0.650842), btScalar(-0.087756), btScalar(0.753983)), + btVector3(btScalar(0.740411), btScalar(-0.497404), btScalar(0.451830)), + btVector3(btScalar(0.726946), btScalar(-0.619890), btScalar(0.295093)), + btVector3(btScalar(0.637768), btScalar(-0.313092), btScalar(0.703624)), + btVector3(btScalar(0.678942), btScalar(-0.722934), btScalar(0.126645)), + btVector3(btScalar(0.489072), btScalar(-0.867195), btScalar(-0.092942)), + btVector3(btScalar(0.622742), btScalar(-0.757541), btScalar(-0.194636)), + btVector3(btScalar(0.596788), btScalar(-0.693576), btScalar(-0.403098)), + btVector3(btScalar(0.550150), btScalar(-0.582172), btScalar(-0.598287)), + btVector3(btScalar(0.474436), btScalar(-0.429745), btScalar(-0.768101)), + btVector3(btScalar(0.372574), btScalar(-0.246016), btScalar(-0.894583)), + btVector3(btScalar(0.480095), btScalar(-0.026513), btScalar(-0.876626)), + btVector3(btScalar(0.352474), btScalar(0.177242), btScalar(-0.918787)), + btVector3(btScalar(0.441848), btScalar(0.374386), btScalar(-0.814946)), + btVector3(btScalar(0.492389), btScalar(0.582223), btScalar(-0.646693)), + btVector3(btScalar(0.343498), btScalar(0.866080), btScalar(-0.362693)), + btVector3(btScalar(0.362036), btScalar(0.745149), btScalar(-0.559639)), + btVector3(btScalar(0.334131), btScalar(0.937044), btScalar(-0.099774)), + btVector3(btScalar(0.486925), btScalar(0.871718), btScalar(0.052473)), + btVector3(btScalar(0.452776), btScalar(0.845665), btScalar(0.281820)), + btVector3(btScalar(0.399503), btScalar(0.771785), btScalar(0.494576)), + btVector3(btScalar(0.296469), btScalar(0.673018), btScalar(0.677469)), + btVector3(btScalar(0.392088), btScalar(0.479179), btScalar(0.785213)), + btVector3(btScalar(0.452190), btScalar(0.252094), btScalar(0.855286)), + btVector3(btScalar(0.478339), btScalar(0.013149), btScalar(0.877928)), + btVector3(btScalar(0.481656), btScalar(-0.219380), btScalar(0.848259)), + btVector3(btScalar(0.615327), btScalar(-0.494293), btScalar(0.613837)), + btVector3(btScalar(0.594642), btScalar(-0.650414), btScalar(0.472325)), + btVector3(btScalar(0.562249), btScalar(-0.771345), btScalar(0.297631)), + btVector3(btScalar(0.467411), btScalar(-0.437133), btScalar(0.768231)), + btVector3(btScalar(0.519513), btScalar(-0.847947), btScalar(0.103808)), + btVector3(btScalar(0.297640), btScalar(-0.938159), btScalar(-0.176288)), + btVector3(btScalar(0.446727), btScalar(-0.838615), btScalar(-0.311359)), + btVector3(btScalar(0.331790), btScalar(-0.942437), btScalar(0.040762)), + btVector3(btScalar(0.413358), btScalar(-0.748403), btScalar(-0.518259)), + btVector3(btScalar(0.347596), btScalar(-0.621640), btScalar(-0.701737)), + btVector3(btScalar(0.249831), btScalar(-0.456186), btScalar(-0.853984)), + btVector3(btScalar(0.131772), btScalar(-0.262931), btScalar(-0.955678)), + btVector3(btScalar(0.247099), btScalar(-0.042261), btScalar(-0.967975)), + btVector3(btScalar(0.113624), btScalar(0.165965), btScalar(-0.979491)), + btVector3(btScalar(0.217438), btScalar(0.374580), btScalar(-0.901220)), + btVector3(btScalar(0.307983), btScalar(0.554615), btScalar(-0.772786)), + btVector3(btScalar(0.166702), btScalar(0.953181), btScalar(-0.252021)), + btVector3(btScalar(0.172751), btScalar(0.844499), btScalar(-0.506743)), + btVector3(btScalar(0.177630), btScalar(0.711125), btScalar(-0.679876)), + btVector3(btScalar(0.120064), btScalar(0.992260), btScalar(-0.030482)), + btVector3(btScalar(0.289640), btScalar(0.949098), btScalar(0.122546)), + btVector3(btScalar(0.239879), btScalar(0.909047), btScalar(0.340377)), + btVector3(btScalar(0.181142), btScalar(0.821363), btScalar(0.540641)), + btVector3(btScalar(0.066986), btScalar(0.719097), btScalar(0.691327)), + btVector3(btScalar(0.156750), btScalar(0.545478), btScalar(0.823079)), + btVector3(btScalar(0.236172), btScalar(0.342306), btScalar(0.909353)), + btVector3(btScalar(0.277541), btScalar(0.112693), btScalar(0.953856)), + btVector3(btScalar(0.295299), btScalar(-0.121974), btScalar(0.947415)), + btVector3(btScalar(0.287883), btScalar(-0.349254), btScalar(0.891591)), + btVector3(btScalar(0.437165), btScalar(-0.634666), btScalar(0.636869)), + btVector3(btScalar(0.407113), btScalar(-0.784954), btScalar(0.466664)), + btVector3(btScalar(0.375111), btScalar(-0.888193), btScalar(0.264839)), + btVector3(btScalar(0.275394), btScalar(-0.560591), btScalar(0.780723)), + btVector3(btScalar(0.122015), btScalar(-0.992209), btScalar(-0.024821)), + btVector3(btScalar(0.087866), btScalar(-0.966156), btScalar(-0.241676)), + btVector3(btScalar(0.239489), btScalar(-0.885665), btScalar(-0.397437)), + btVector3(btScalar(0.167287), btScalar(-0.965184), btScalar(0.200817)), + btVector3(btScalar(0.201632), btScalar(-0.776789), btScalar(-0.596335)), + btVector3(btScalar(0.122015), btScalar(-0.637971), btScalar(-0.760098)), + btVector3(btScalar(0.008054), btScalar(-0.464741), btScalar(-0.885214)), + btVector3(btScalar(-0.116054), btScalar(-0.271096), btScalar(-0.955482)), + btVector3(btScalar(-0.000727), btScalar(-0.056065), btScalar(-0.998424)), + btVector3(btScalar(-0.134007), btScalar(0.152939), btScalar(-0.978905)), + btVector3(btScalar(-0.025900), btScalar(0.366026), btScalar(-0.930108)), + btVector3(btScalar(0.081231), btScalar(0.557337), btScalar(-0.826072)), + btVector3(btScalar(-0.002874), btScalar(0.917213), btScalar(-0.398023)), + btVector3(btScalar(-0.050683), btScalar(0.981761), btScalar(-0.182534)), + btVector3(btScalar(-0.040536), btScalar(0.710153), btScalar(-0.702713)), + btVector3(btScalar(-0.139081), btScalar(0.827973), btScalar(-0.543048)), + btVector3(btScalar(-0.101029), btScalar(0.994010), btScalar(0.041152)), + btVector3(btScalar(0.069328), btScalar(0.978067), btScalar(0.196133)), + btVector3(btScalar(0.023860), btScalar(0.911380), btScalar(0.410645)), + btVector3(btScalar(-0.153521), btScalar(0.736789), btScalar(0.658145)), + btVector3(btScalar(-0.070002), btScalar(0.591750), btScalar(0.802780)), + btVector3(btScalar(0.002590), btScalar(0.312948), btScalar(0.949562)), + btVector3(btScalar(0.090988), btScalar(-0.020680), btScalar(0.995627)), + btVector3(btScalar(0.088842), btScalar(-0.250099), btScalar(0.964006)), + btVector3(btScalar(0.083378), btScalar(-0.470185), btScalar(0.878318)), + btVector3(btScalar(0.240074), btScalar(-0.749764), btScalar(0.616374)), + btVector3(btScalar(0.210803), btScalar(-0.885860), btScalar(0.412987)), + btVector3(btScalar(0.077524), btScalar(-0.660524), btScalar(0.746565)), + btVector3(btScalar(-0.096736), btScalar(-0.990070), btScalar(-0.100945)), + btVector3(btScalar(-0.052634), btScalar(-0.990264), btScalar(0.127426)), + btVector3(btScalar(-0.106102), btScalar(-0.938354), btScalar(-0.328340)), + btVector3(btScalar(0.013323), btScalar(-0.863112), btScalar(-0.504596)), + btVector3(btScalar(-0.002093), btScalar(-0.936993), btScalar(0.349161)), + btVector3(btScalar(-0.106297), btScalar(-0.636610), btScalar(-0.763612)), + btVector3(btScalar(-0.229430), btScalar(-0.463769), btScalar(-0.855546)), + btVector3(btScalar(-0.245236), btScalar(-0.066175), btScalar(-0.966999)), + btVector3(btScalar(-0.351587), btScalar(-0.270513), btScalar(-0.896145)), + btVector3(btScalar(-0.370906), btScalar(0.133108), btScalar(-0.918982)), + btVector3(btScalar(-0.264360), btScalar(0.346000), btScalar(-0.900049)), + btVector3(btScalar(-0.151375), btScalar(0.543728), btScalar(-0.825291)), + btVector3(btScalar(-0.218697), btScalar(0.912741), btScalar(-0.344346)), + btVector3(btScalar(-0.274507), btScalar(0.953764), btScalar(-0.121635)), + btVector3(btScalar(-0.259677), btScalar(0.692266), btScalar(-0.673044)), + btVector3(btScalar(-0.350416), btScalar(0.798810), btScalar(-0.488786)), + btVector3(btScalar(-0.320170), btScalar(0.941127), btScalar(0.108297)), + btVector3(btScalar(-0.147667), btScalar(0.952792), btScalar(0.265034)), + btVector3(btScalar(-0.188061), btScalar(0.860636), btScalar(0.472910)), + btVector3(btScalar(-0.370906), btScalar(0.739900), btScalar(0.560941)), + btVector3(btScalar(-0.297143), btScalar(0.585334), btScalar(0.754178)), + btVector3(btScalar(-0.189622), btScalar(0.428241), btScalar(0.883393)), + btVector3(btScalar(-0.091272), btScalar(0.098695), btScalar(0.990747)), + btVector3(btScalar(-0.256945), btScalar(0.228375), btScalar(0.938827)), + btVector3(btScalar(-0.111761), btScalar(-0.133251), btScalar(0.984696)), + btVector3(btScalar(-0.118006), btScalar(-0.356253), btScalar(0.926725)), + btVector3(btScalar(-0.119372), btScalar(-0.563896), btScalar(0.817029)), + btVector3(btScalar(0.041228), btScalar(-0.833949), btScalar(0.550010)), + btVector3(btScalar(-0.121909), btScalar(-0.736543), btScalar(0.665172)), + btVector3(btScalar(-0.307681), btScalar(-0.931160), btScalar(-0.195026)), + btVector3(btScalar(-0.283679), btScalar(-0.957990), btScalar(0.041348)), + btVector3(btScalar(-0.227284), btScalar(-0.935243), btScalar(0.270890)), + btVector3(btScalar(-0.293436), btScalar(-0.858252), btScalar(-0.420860)), + btVector3(btScalar(-0.175767), btScalar(-0.780677), btScalar(-0.599262)), + btVector3(btScalar(-0.170108), btScalar(-0.858835), btScalar(0.482865)), + btVector3(btScalar(-0.332854), btScalar(-0.635055), btScalar(-0.696857)), + btVector3(btScalar(-0.447791), btScalar(-0.445299), btScalar(-0.775128)), + btVector3(btScalar(-0.470622), btScalar(-0.074146), btScalar(-0.879164)), + btVector3(btScalar(-0.639417), btScalar(-0.340505), btScalar(-0.689049)), + btVector3(btScalar(-0.598438), btScalar(0.104722), btScalar(-0.794256)), + btVector3(btScalar(-0.488575), btScalar(0.307699), btScalar(-0.816313)), + btVector3(btScalar(-0.379882), btScalar(0.513592), btScalar(-0.769077)), + btVector3(btScalar(-0.425740), btScalar(0.862775), btScalar(-0.272516)), + btVector3(btScalar(-0.480769), btScalar(0.875412), btScalar(-0.048439)), + btVector3(btScalar(-0.467890), btScalar(0.648716), btScalar(-0.600043)), + btVector3(btScalar(-0.543799), btScalar(0.730956), btScalar(-0.411881)), + btVector3(btScalar(-0.516284), btScalar(0.838277), btScalar(0.174076)), + btVector3(btScalar(-0.353343), btScalar(0.876384), btScalar(0.326519)), + btVector3(btScalar(-0.572875), btScalar(0.614497), btScalar(0.542007)), + btVector3(btScalar(-0.503600), btScalar(0.497261), btScalar(0.706161)), + btVector3(btScalar(-0.530920), btScalar(0.754870), btScalar(0.384685)), + btVector3(btScalar(-0.395884), btScalar(0.366414), btScalar(0.841818)), + btVector3(btScalar(-0.300656), btScalar(0.001678), btScalar(0.953661)), + btVector3(btScalar(-0.461060), btScalar(0.146912), btScalar(0.875000)), + btVector3(btScalar(-0.315486), btScalar(-0.232212), btScalar(0.919893)), + btVector3(btScalar(-0.323682), btScalar(-0.449187), btScalar(0.832644)), + btVector3(btScalar(-0.318999), btScalar(-0.639527), btScalar(0.699134)), + btVector3(btScalar(-0.496771), btScalar(-0.866029), btScalar(-0.055271)), + btVector3(btScalar(-0.496771), btScalar(-0.816257), btScalar(-0.294377)), + btVector3(btScalar(-0.456377), btScalar(-0.869528), btScalar(0.188130)), + btVector3(btScalar(-0.380858), btScalar(-0.827144), btScalar(0.412792)), + btVector3(btScalar(-0.449352), btScalar(-0.727405), btScalar(-0.518259)), + btVector3(btScalar(-0.570533), btScalar(-0.551064), btScalar(-0.608632)), + btVector3(btScalar(-0.656394), btScalar(-0.118280), btScalar(-0.744874)), + btVector3(btScalar(-0.756696), btScalar(-0.438105), btScalar(-0.484882)), + btVector3(btScalar(-0.801773), btScalar(-0.204798), btScalar(-0.561005)), + btVector3(btScalar(-0.785186), btScalar(0.038618), btScalar(-0.617805)), + btVector3(btScalar(-0.709082), btScalar(0.262399), btScalar(-0.654306)), + btVector3(btScalar(-0.583412), btScalar(0.462265), btScalar(-0.667383)), + btVector3(btScalar(-0.616001), btScalar(0.761286), btScalar(-0.201272)), + btVector3(btScalar(-0.660687), btScalar(0.750204), btScalar(0.020072)), + btVector3(btScalar(-0.744987), btScalar(0.435823), btScalar(-0.504791)), + btVector3(btScalar(-0.713765), btScalar(0.605554), btScalar(-0.351373)), + btVector3(btScalar(-0.686251), btScalar(0.687600), btScalar(0.236927)), + btVector3(btScalar(-0.680201), btScalar(0.429407), btScalar(0.593732)), + btVector3(btScalar(-0.733474), btScalar(0.546450), btScalar(0.403814)), + btVector3(btScalar(-0.591023), btScalar(0.292923), btScalar(0.751445)), + btVector3(btScalar(-0.500283), btScalar(-0.080757), btScalar(0.861922)), + btVector3(btScalar(-0.643710), btScalar(0.070115), btScalar(0.761985)), + btVector3(btScalar(-0.506332), btScalar(-0.308425), btScalar(0.805122)), + btVector3(btScalar(-0.503015), btScalar(-0.509847), btScalar(0.697573)), + btVector3(btScalar(-0.482525), btScalar(-0.682105), btScalar(0.549229)), + btVector3(btScalar(-0.680396), btScalar(-0.716323), btScalar(-0.153451)), + btVector3(btScalar(-0.658346), btScalar(-0.746264), btScalar(0.097562)), + btVector3(btScalar(-0.653272), btScalar(-0.646915), btScalar(-0.392948)), + btVector3(btScalar(-0.590828), btScalar(-0.732655), btScalar(0.337645)), + btVector3(btScalar(-0.819140), btScalar(-0.518013), btScalar(-0.246166)), + btVector3(btScalar(-0.900513), btScalar(-0.282178), btScalar(-0.330487)), + btVector3(btScalar(-0.914953), btScalar(-0.028652), btScalar(-0.402122)), + btVector3(btScalar(-0.859924), btScalar(0.220209), btScalar(-0.459898)), + btVector3(btScalar(-0.777185), btScalar(0.613720), btScalar(-0.137836)), + btVector3(btScalar(-0.805285), btScalar(0.586889), btScalar(0.082728)), + btVector3(btScalar(-0.872413), btScalar(0.406077), btScalar(-0.271735)), + btVector3(btScalar(-0.859339), btScalar(0.448072), btScalar(0.246101)), + btVector3(btScalar(-0.757671), btScalar(0.216320), btScalar(0.615594)), + btVector3(btScalar(-0.826165), btScalar(0.348139), btScalar(0.442851)), + btVector3(btScalar(-0.671810), btScalar(-0.162803), btScalar(0.722557)), + btVector3(btScalar(-0.796504), btScalar(-0.004543), btScalar(0.604468)), + btVector3(btScalar(-0.676298), btScalar(-0.378223), btScalar(0.631794)), + btVector3(btScalar(-0.668883), btScalar(-0.558258), btScalar(0.490673)), + btVector3(btScalar(-0.821287), btScalar(-0.570118), btScalar(0.006994)), + btVector3(btScalar(-0.767428), btScalar(-0.587810), btScalar(0.255470)), + btVector3(btScalar(-0.933296), btScalar(-0.349837), btScalar(-0.079865)), + btVector3(btScalar(-0.982667), btScalar(-0.100393), btScalar(-0.155208)), + btVector3(btScalar(-0.961396), btScalar(0.160910), btScalar(-0.222938)), + btVector3(btScalar(-0.934858), btScalar(0.354555), btScalar(-0.006864)), + btVector3(btScalar(-0.941687), btScalar(0.229736), btScalar(0.245711)), + btVector3(btScalar(-0.884317), btScalar(0.131552), btScalar(0.447536)), + btVector3(btScalar(-0.810359), btScalar(-0.219769), btScalar(0.542788)), + btVector3(btScalar(-0.915929), btScalar(-0.210048), btScalar(0.341743)), + btVector3(btScalar(-0.816799), btScalar(-0.407192), btScalar(0.408303)), + btVector3(btScalar(-0.903050), btScalar(-0.392416), btScalar(0.174076)), + btVector3(btScalar(-0.980325), btScalar(-0.170969), btScalar(0.096586)), + btVector3(btScalar(-0.995936), btScalar(0.084891), btScalar(0.029441)), + btVector3(btScalar(-0.960031), btScalar(0.002650), btScalar(0.279283)), + }; + static btVector3 sUnitSpherePoints[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)) + }; + if (highres) + return sUnitSpherePointsHighres; + return sUnitSpherePoints; +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h b/extern/bullet/src/BulletCollision/CollisionShapes/btShapeHull.h similarity index 95% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btShapeHull.h index e959f198b69a..78ea4b65011f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btShapeHull.h @@ -34,7 +34,7 @@ ATTRIBUTE_ALIGNED16(class) btShapeHull unsigned int m_numIndices; const btConvexShape* m_shape; - static btVector3* getUnitSpherePoints(); + static btVector3* getUnitSpherePoints(int highres=0); public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -42,7 +42,7 @@ ATTRIBUTE_ALIGNED16(class) btShapeHull btShapeHull (const btConvexShape* shape); ~btShapeHull (); - bool buildHull (btScalar margin); + bool buildHull (btScalar margin, int highres=0); int numTriangles () const; int numVertices () const; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btSphereShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btSphereShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btSphereShape.h similarity index 96% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btSphereShape.h index b192efeeb8df..50561f7f54fd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btSphereShape.h @@ -29,8 +29,11 @@ ATTRIBUTE_ALIGNED16(class) btSphereShape : public btConvexInternalShape btSphereShape (btScalar radius) : btConvexInternalShape () { m_shapeType = SPHERE_SHAPE_PROXYTYPE; + m_localScaling.setValue(1.0, 1.0, 1.0); + m_implicitShapeDimensions.setZero(); m_implicitShapeDimensions.setX(radius); m_collisionMargin = radius; + m_padding = 0; } virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp similarity index 99% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp index d17141e3f205..b5e0e716d487 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp @@ -69,8 +69,6 @@ void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const //tangentDir0/tangentDir1 can be precalculated btPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1); - btVector3 supVertex0,supVertex1; - btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal; btVector3 triangle[3]; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h similarity index 95% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h index e6e328839590..5e9eccc77d9e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h @@ -94,7 +94,13 @@ SIMD_FORCE_INLINE const char* btStaticPlaneShape::serialize(void* dataBuffer, bt m_localScaling.serializeFloat(planeData->m_localScaling); m_planeNormal.serializeFloat(planeData->m_planeNormal); planeData->m_planeConstant = float(m_planeConstant); - + + // Fill padding with zeros to appease msan. + planeData->m_pad[0] = 0; + planeData->m_pad[1] = 0; + planeData->m_pad[2] = 0; + planeData->m_pad[3] = 0; + return "btStaticPlaneShapeData"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp similarity index 97% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp index b3d449676b9a..78ddeb37047c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp @@ -293,6 +293,9 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s tmpIndices[gfxindex].m_values[0] = tri_indices[0]; tmpIndices[gfxindex].m_values[1] = tri_indices[1]; tmpIndices[gfxindex].m_values[2] = tri_indices[2]; + // Fill padding with zeros to appease msan. + tmpIndices[gfxindex].m_pad[0] = 0; + tmpIndices[gfxindex].m_pad[1] = 0; } serializer->finalizeChunk(chunk,"btShortIntIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); } @@ -311,6 +314,8 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s tmpIndices[gfxindex].m_values[0] = tri_indices[0]; tmpIndices[gfxindex].m_values[1] = tri_indices[1]; tmpIndices[gfxindex].m_values[2] = tri_indices[2]; + // Fill padding with zeros to appease msan. + tmpIndices[gfxindex].m_pad = 0; } serializer->finalizeChunk(chunk,"btCharIndexTripletData",BT_ARRAY_CODE,(void*)chunk->m_oldPtr); } @@ -375,6 +380,8 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s serializer->finalizeChunk(chunk,"btMeshPartData",BT_ARRAY_CODE,chunk->m_oldPtr); } + // Fill padding with zeros to appease msan. + memset(trimeshData->m_padding, 0, sizeof(trimeshData->m_padding)); m_scaling.serializeFloat(trimeshData->m_scaling); return "btStridingMeshInterfaceData"; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h b/extern/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btTetrahedronShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btTetrahedronShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleBuffer.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleBuffer.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleCallback.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleCallback.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h similarity index 98% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index 9e1544e87a4c..b7a6f74361f1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -63,7 +63,7 @@ typedef btAlignedObjectArray IndexedMeshArray; ///The btTriangleIndexVertexArray allows to access multiple triangle meshes, by indexing into existing triangle/index arrays. ///Additional meshes can be added using addIndexedMesh -///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays. +///No duplicate is made of the vertex/index data, it only indexes into external vertex/index arrays. ///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray. ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshInterface { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h similarity index 98% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h index 17deef89d377..642758959011 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h +++ b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h @@ -195,6 +195,13 @@ SIMD_FORCE_INLINE const char* btTriangleInfoMap::serialize(void* dataBuffer, btS serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*) &m_keyArray[0]); } + + // Fill padding with zeros to appease msan. + tmapData->m_padding[0] = 0; + tmapData->m_padding[1] = 0; + tmapData->m_padding[2] = 0; + tmapData->m_padding[3] = 0; + return "btTriangleInfoMapData"; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleMesh.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleMesh.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btTriangleShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btTriangleShape.h diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp b/extern/bullet/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp rename to extern/bullet/src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h b/extern/bullet/src/BulletCollision/CollisionShapes/btUniformScalingShape.h similarity index 100% rename from extern/bullet2/src/BulletCollision/CollisionShapes/btUniformScalingShape.h rename to extern/bullet/src/BulletCollision/CollisionShapes/btUniformScalingShape.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h b/extern/bullet/src/BulletCollision/Gimpact/btBoxCollision.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/btBoxCollision.h rename to extern/bullet/src/BulletCollision/Gimpact/btBoxCollision.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h b/extern/bullet/src/BulletCollision/Gimpact/btClipPolygon.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/btClipPolygon.h rename to extern/bullet/src/BulletCollision/Gimpact/btClipPolygon.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btCompoundFromGimpact.h b/extern/bullet/src/BulletCollision/Gimpact/btCompoundFromGimpact.h similarity index 86% rename from extern/bullet2/src/BulletCollision/Gimpact/btCompoundFromGimpact.h rename to extern/bullet/src/BulletCollision/Gimpact/btCompoundFromGimpact.h index 02f8b678a45d..19f7ecddd0e7 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btCompoundFromGimpact.h +++ b/extern/bullet/src/BulletCollision/Gimpact/btCompoundFromGimpact.h @@ -5,6 +5,22 @@ #include "btGImpactShape.h" #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" +ATTRIBUTE_ALIGNED16(class) btCompoundFromGimpactShape : public btCompoundShape +{ +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + virtual ~btCompoundFromGimpactShape() + { + /*delete all the btBU_Simplex1to4 ChildShapes*/ + for (int i = 0; i < m_children.size(); i++) + { + delete m_children[i].m_childShape; + } + } + +}; + struct MyCallback : public btTriangleRaycastCallback { int m_ignorePart; @@ -77,7 +93,7 @@ struct MyCallback : public btTriangleRaycastCallback btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimpactMesh, btScalar depth) { - btCompoundShape* colShape = new btCompoundShape(); + btCompoundShape* colShape = new btCompoundFromGimpactShape(); btTransform tr; tr.setIdentity(); @@ -90,4 +106,4 @@ btCompoundShape* btCreateCompoundFromGimpactShape(const btGImpactMeshShape* gimp return colShape; } -#endif //BT_COMPOUND_FROM_GIMPACT \ No newline at end of file +#endif //BT_COMPOUND_FROM_GIMPACT diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp b/extern/bullet/src/BulletCollision/Gimpact/btContactProcessing.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp rename to extern/bullet/src/BulletCollision/Gimpact/btContactProcessing.cpp diff --git a/extern/bullet/src/BulletCollision/Gimpact/btContactProcessing.h b/extern/bullet/src/BulletCollision/Gimpact/btContactProcessing.h new file mode 100644 index 000000000000..d1027dbe67e4 --- /dev/null +++ b/extern/bullet/src/BulletCollision/Gimpact/btContactProcessing.h @@ -0,0 +1,65 @@ +#ifndef BT_CONTACT_H_INCLUDED +#define BT_CONTACT_H_INCLUDED + +/*! \file gim_contact.h +\author Francisco Leon Najera +*/ +/* +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@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 "LinearMath/btTransform.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "btTriangleShapeEx.h" +#include "btContactProcessingStructs.h" + +class btContactArray:public btAlignedObjectArray +{ +public: + btContactArray() + { + reserve(64); + } + + SIMD_FORCE_INLINE void push_contact( + const btVector3 &point,const btVector3 & normal, + btScalar depth, int feature1, int feature2) + { + push_back( GIM_CONTACT(point,normal,depth,feature1,feature2) ); + } + + SIMD_FORCE_INLINE void push_triangle_contacts( + const GIM_TRIANGLE_CONTACT & tricontact, + int feature1,int feature2) + { + for(int i = 0;i -{ -public: - btContactArray() - { - reserve(64); - } - - SIMD_FORCE_INLINE void push_contact( - const btVector3 &point,const btVector3 & normal, - btScalar depth, int feature1, int feature2) - { - push_back( GIM_CONTACT(point,normal,depth,feature1,feature2) ); - } - - SIMD_FORCE_INLINE void push_triangle_contacts( - const GIM_TRIANGLE_CONTACT & tricontact, - int feature1,int feature2) - { - for(int i = 0;i @@ -74,59 +50,6 @@ class btPairSet: public btAlignedObjectArray } }; - -///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box -struct GIM_BVH_DATA -{ - btAABB m_bound; - int m_data; -}; - -//! Node Structure for trees -class GIM_BVH_TREE_NODE -{ -public: - btAABB m_bound; -protected: - int m_escapeIndexOrDataIndex; -public: - GIM_BVH_TREE_NODE() - { - m_escapeIndexOrDataIndex = 0; - } - - SIMD_FORCE_INLINE bool isLeafNode() const - { - //skipindex is negative (internal node), triangleindex >=0 (leafnode) - return (m_escapeIndexOrDataIndex>=0); - } - - SIMD_FORCE_INLINE int getEscapeIndex() const - { - //btAssert(m_escapeIndexOrDataIndex < 0); - return -m_escapeIndexOrDataIndex; - } - - SIMD_FORCE_INLINE void setEscapeIndex(int index) - { - m_escapeIndexOrDataIndex = -index; - } - - SIMD_FORCE_INLINE int getDataIndex() const - { - //btAssert(m_escapeIndexOrDataIndex >= 0); - - return m_escapeIndexOrDataIndex; - } - - SIMD_FORCE_INLINE void setDataIndex(int index) - { - m_escapeIndexOrDataIndex = index; - } - -}; - - class GIM_BVH_DATA_ARRAY:public btAlignedObjectArray { }; @@ -392,5 +315,4 @@ class btGImpactBvh btPairSet & collision_pairs); }; - #endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/extern/bullet/src/BulletCollision/Gimpact/btGImpactBvhStructs.h b/extern/bullet/src/BulletCollision/Gimpact/btGImpactBvhStructs.h new file mode 100644 index 000000000000..9342a572d09c --- /dev/null +++ b/extern/bullet/src/BulletCollision/Gimpact/btGImpactBvhStructs.h @@ -0,0 +1,105 @@ +#ifndef GIM_BOX_SET_STRUCT_H_INCLUDED +#define GIM_BOX_SET_STRUCT_H_INCLUDED + +/*! \file gim_box_set.h +\author Francisco Leon Najera +*/ +/* +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@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 "LinearMath/btAlignedObjectArray.h" + +#include "btBoxCollision.h" +#include "btTriangleShapeEx.h" + +//! Overlapping pair +struct GIM_PAIR +{ + int m_index1; + int m_index2; + GIM_PAIR() + {} + + GIM_PAIR(const GIM_PAIR & p) + { + m_index1 = p.m_index1; + m_index2 = p.m_index2; + } + + GIM_PAIR(int index1, int index2) + { + m_index1 = index1; + m_index2 = index2; + } +}; + +///GIM_BVH_DATA is an internal GIMPACT collision structure to contain axis aligned bounding box +struct GIM_BVH_DATA +{ + btAABB m_bound; + int m_data; +}; + +//! Node Structure for trees +class GIM_BVH_TREE_NODE +{ +public: + btAABB m_bound; +protected: + int m_escapeIndexOrDataIndex; +public: + GIM_BVH_TREE_NODE() + { + m_escapeIndexOrDataIndex = 0; + } + + SIMD_FORCE_INLINE bool isLeafNode() const + { + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (m_escapeIndexOrDataIndex>=0); + } + + SIMD_FORCE_INLINE int getEscapeIndex() const + { + //btAssert(m_escapeIndexOrDataIndex < 0); + return -m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setEscapeIndex(int index) + { + m_escapeIndexOrDataIndex = -index; + } + + SIMD_FORCE_INLINE int getDataIndex() const + { + //btAssert(m_escapeIndexOrDataIndex >= 0); + + return m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setDataIndex(int index) + { + m_escapeIndexOrDataIndex = index; + } + +}; + +#endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp b/extern/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp rename to extern/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h b/extern/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h similarity index 99% rename from extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h rename to extern/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h index f85a94cb4c7d..3e5675f729d4 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h +++ b/extern/bullet/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h @@ -122,7 +122,7 @@ class btGImpactCollisionAlgorithm : public btActivatingCollisionAlgorithm checkManifold(body0Wrap,body1Wrap); btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm( - body0Wrap,body1Wrap,getLastManifold()); + body0Wrap,body1Wrap,getLastManifold(), BT_CONTACT_POINT_ALGORITHMS); return convex_algorithm ; } diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h b/extern/bullet/src/BulletCollision/Gimpact/btGImpactMassUtil.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/btGImpactMassUtil.h rename to extern/bullet/src/BulletCollision/Gimpact/btGImpactMassUtil.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/extern/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp rename to extern/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h b/extern/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h similarity index 84% rename from extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h rename to extern/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h index e6e52fff4c07..42e5520fc03a 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h +++ b/extern/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h @@ -26,73 +26,7 @@ subject to the following restrictions: #include "btGImpactBvh.h" #include "btQuantization.h" - - - - - -///btQuantizedBvhNode is a compressed aabb node, 16 bytes. -///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). -ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE -{ - //12 bytes - unsigned short int m_quantizedAabbMin[3]; - unsigned short int m_quantizedAabbMax[3]; - //4 bytes - int m_escapeIndexOrDataIndex; - - BT_QUANTIZED_BVH_NODE() - { - m_escapeIndexOrDataIndex = 0; - } - - SIMD_FORCE_INLINE bool isLeafNode() const - { - //skipindex is negative (internal node), triangleindex >=0 (leafnode) - return (m_escapeIndexOrDataIndex>=0); - } - - SIMD_FORCE_INLINE int getEscapeIndex() const - { - //btAssert(m_escapeIndexOrDataIndex < 0); - return -m_escapeIndexOrDataIndex; - } - - SIMD_FORCE_INLINE void setEscapeIndex(int index) - { - m_escapeIndexOrDataIndex = -index; - } - - SIMD_FORCE_INLINE int getDataIndex() const - { - //btAssert(m_escapeIndexOrDataIndex >= 0); - - return m_escapeIndexOrDataIndex; - } - - SIMD_FORCE_INLINE void setDataIndex(int index) - { - m_escapeIndexOrDataIndex = index; - } - - SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp( - unsigned short * quantizedMin,unsigned short * quantizedMax) const - { - if(m_quantizedAabbMin[0] > quantizedMax[0] || - m_quantizedAabbMax[0] < quantizedMin[0] || - m_quantizedAabbMin[1] > quantizedMax[1] || - m_quantizedAabbMax[1] < quantizedMin[1] || - m_quantizedAabbMin[2] > quantizedMax[2] || - m_quantizedAabbMax[2] < quantizedMin[2]) - { - return false; - } - return true; - } - -}; - - +#include "btGImpactQuantizedBvhStructs.h" class GIM_QUANTIZED_BVH_NODE_ARRAY:public btAlignedObjectArray { @@ -368,5 +302,4 @@ class btGImpactQuantizedBvh btPairSet & collision_pairs); }; - #endif // GIM_BOXPRUNING_H_INCLUDED diff --git a/extern/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h b/extern/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h new file mode 100644 index 000000000000..7dd5a1b9d01e --- /dev/null +++ b/extern/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvhStructs.h @@ -0,0 +1,91 @@ +#ifndef GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED +#define GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED + +/*! \file btGImpactQuantizedBvh.h +\author Francisco Leon Najera +*/ +/* +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@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 "btGImpactBvh.h" +#include "btQuantization.h" + +///btQuantizedBvhNode is a compressed aabb node, 16 bytes. +///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). +ATTRIBUTE_ALIGNED16 (struct) BT_QUANTIZED_BVH_NODE +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes + int m_escapeIndexOrDataIndex; + + BT_QUANTIZED_BVH_NODE() + { + m_escapeIndexOrDataIndex = 0; + } + + SIMD_FORCE_INLINE bool isLeafNode() const + { + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (m_escapeIndexOrDataIndex>=0); + } + + SIMD_FORCE_INLINE int getEscapeIndex() const + { + //btAssert(m_escapeIndexOrDataIndex < 0); + return -m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setEscapeIndex(int index) + { + m_escapeIndexOrDataIndex = -index; + } + + SIMD_FORCE_INLINE int getDataIndex() const + { + //btAssert(m_escapeIndexOrDataIndex >= 0); + + return m_escapeIndexOrDataIndex; + } + + SIMD_FORCE_INLINE void setDataIndex(int index) + { + m_escapeIndexOrDataIndex = index; + } + + SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp( + unsigned short * quantizedMin,unsigned short * quantizedMax) const + { + if(m_quantizedAabbMin[0] > quantizedMax[0] || + m_quantizedAabbMax[0] < quantizedMin[0] || + m_quantizedAabbMin[1] > quantizedMax[1] || + m_quantizedAabbMax[1] < quantizedMin[1] || + m_quantizedAabbMin[2] > quantizedMax[2] || + m_quantizedAabbMax[2] < quantizedMin[2]) + { + return false; + } + return true; + } + +}; + +#endif // GIM_QUANTIZED_SET_STRUCTS_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp b/extern/bullet/src/BulletCollision/Gimpact/btGImpactShape.cpp similarity index 73% rename from extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp rename to extern/bullet/src/BulletCollision/Gimpact/btGImpactShape.cpp index ac8efdf3833d..30c85e3fffd1 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.cpp +++ b/extern/bullet/src/BulletCollision/Gimpact/btGImpactShape.cpp @@ -23,6 +23,59 @@ subject to the following restrictions: #include "btGImpactMassUtil.h" +btGImpactMeshShapePart::btGImpactMeshShapePart( btStridingMeshInterface * meshInterface, int part ) +{ + // moved from .h to .cpp because of conditional compilation + // (The setting of BT_THREADSAFE may differ between various cpp files, so it is best to + // avoid using it in h files) + m_primitive_manager.m_meshInterface = meshInterface; + m_primitive_manager.m_part = part; + m_box_set.setPrimitiveManager( &m_primitive_manager ); +#if BT_THREADSAFE + // If threadsafe is requested, this object uses a different lock/unlock + // model with the btStridingMeshInterface -- lock once when the object is constructed + // and unlock once in the destructor. + // The other way of locking and unlocking for each collision check in the narrowphase + // is not threadsafe. Note these are not thread-locks, they are calls to the meshInterface's + // getLockedReadOnlyVertexIndexBase virtual function, which by default just returns a couple of + // pointers. In theory a client could override the lock function to do all sorts of + // things like reading data from GPU memory, or decompressing data on the fly, but such things + // do not seem all that likely or useful, given the performance cost. + m_primitive_manager.lock(); +#endif +} + +btGImpactMeshShapePart::~btGImpactMeshShapePart() +{ + // moved from .h to .cpp because of conditional compilation +#if BT_THREADSAFE + m_primitive_manager.unlock(); +#endif +} + +void btGImpactMeshShapePart::lockChildShapes() const +{ + // moved from .h to .cpp because of conditional compilation +#if ! BT_THREADSAFE + // called in the narrowphase -- not threadsafe! + void * dummy = (void*) ( m_box_set.getPrimitiveManager() ); + TrimeshPrimitiveManager * dummymanager = static_cast( dummy ); + dummymanager->lock(); +#endif +} + +void btGImpactMeshShapePart::unlockChildShapes() const +{ + // moved from .h to .cpp because of conditional compilation +#if ! BT_THREADSAFE + // called in the narrowphase -- not threadsafe! + void * dummy = (void*) ( m_box_set.getPrimitiveManager() ); + TrimeshPrimitiveManager * dummymanager = static_cast( dummy ); + dummymanager->unlock(); +#endif +} + + #define CALC_EXACT_INERTIA 1 diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h b/extern/bullet/src/BulletCollision/Gimpact/btGImpactShape.h similarity index 97% rename from extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h rename to extern/bullet/src/BulletCollision/Gimpact/btGImpactShape.h index 3d1f48d47767..9d7e40562c6a 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactShape.h +++ b/extern/bullet/src/BulletCollision/Gimpact/btGImpactShape.h @@ -722,17 +722,8 @@ class btGImpactMeshShapePart : public btGImpactShapeInterface m_box_set.setPrimitiveManager(&m_primitive_manager); } - - btGImpactMeshShapePart(btStridingMeshInterface * meshInterface, int part) - { - m_primitive_manager.m_meshInterface = meshInterface; - m_primitive_manager.m_part = part; - m_box_set.setPrimitiveManager(&m_primitive_manager); - } - - virtual ~btGImpactMeshShapePart() - { - } + btGImpactMeshShapePart( btStridingMeshInterface * meshInterface, int part ); + virtual ~btGImpactMeshShapePart(); //! if true, then its children must get transforms. virtual bool childrenHasTransform() const @@ -742,19 +733,8 @@ class btGImpactMeshShapePart : public btGImpactShapeInterface //! call when reading child shapes - virtual void lockChildShapes() const - { - void * dummy = (void*)(m_box_set.getPrimitiveManager()); - TrimeshPrimitiveManager * dummymanager = static_cast(dummy); - dummymanager->lock(); - } - - virtual void unlockChildShapes() const - { - void * dummy = (void*)(m_box_set.getPrimitiveManager()); - TrimeshPrimitiveManager * dummymanager = static_cast(dummy); - dummymanager->unlock(); - } + virtual void lockChildShapes() const; + virtual void unlockChildShapes() const; //! Gets the number of children virtual int getNumChildShapes() const diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp b/extern/bullet/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp rename to extern/bullet/src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h b/extern/bullet/src/BulletCollision/Gimpact/btGenericPoolAllocator.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/btGenericPoolAllocator.h rename to extern/bullet/src/BulletCollision/Gimpact/btGenericPoolAllocator.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h b/extern/bullet/src/BulletCollision/Gimpact/btGeometryOperations.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/btGeometryOperations.h rename to extern/bullet/src/BulletCollision/Gimpact/btGeometryOperations.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h b/extern/bullet/src/BulletCollision/Gimpact/btQuantization.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h rename to extern/bullet/src/BulletCollision/Gimpact/btQuantization.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp b/extern/bullet/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp rename to extern/bullet/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h b/extern/bullet/src/BulletCollision/Gimpact/btTriangleShapeEx.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h rename to extern/bullet/src/BulletCollision/Gimpact/btTriangleShapeEx.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h b/extern/bullet/src/BulletCollision/Gimpact/gim_array.h similarity index 99% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_array.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_array.h index 27e6f32fc8b3..cda51a5fcecf 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_array.h +++ b/extern/bullet/src/BulletCollision/Gimpact/gim_array.h @@ -228,7 +228,7 @@ class gim_array inline void push_back_memcpy(const T & obj) { this->growingCheck(); - irr_simd_memcpy(&m_data[m_size],&obj,sizeof(T)); + gim_simd_memcpy(&m_data[m_size],&obj,sizeof(T)); m_size++; } diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h b/extern/bullet/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h similarity index 99% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h index d98051da3d60..0c48cb60fc50 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h +++ b/extern/bullet/src/BulletCollision/Gimpact/gim_basic_geometry_operations.h @@ -41,10 +41,13 @@ email: projectileman@yahoo.com - +#ifndef PLANEDIREPSILON #define PLANEDIREPSILON 0.0000001f -#define PARALELENORMALS 0.000001f +#endif +#ifndef PARALELENORMALS +#define PARALELENORMALS 0.000001f +#endif #define TRIANGLE_NORMAL(v1,v2,v3,n)\ {\ diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h b/extern/bullet/src/BulletCollision/Gimpact/gim_bitset.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_bitset.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_bitset.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h b/extern/bullet/src/BulletCollision/Gimpact/gim_box_collision.h similarity index 99% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_box_collision.h index 9c572638acd4..a051b4fdbf2a 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h +++ b/extern/bullet/src/BulletCollision/Gimpact/gim_box_collision.h @@ -97,6 +97,8 @@ email: projectileman@yahoo.com // return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1); //} +#ifndef TEST_CROSS_EDGE_BOX_MCR + #define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\ {\ const btScalar dir0 = -edge[i_dir_0];\ @@ -113,6 +115,7 @@ email: projectileman@yahoo.com if(pmin>rad || -rad>pmax) return false;\ }\ +#endif #define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\ {\ @@ -190,8 +193,9 @@ class GIM_BOX_BOX_TRANSFORM_CACHE } }; - +#ifndef BOX_PLANE_EPSILON #define BOX_PLANE_EPSILON 0.000001f +#endif //! Axis aligned box class GIM_AABB @@ -571,7 +575,7 @@ class GIM_AABB } }; - +#ifndef BT_BOX_COLLISION_H_INCLUDED //! Compairison of transformation objects SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2) { @@ -582,6 +586,7 @@ SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btT if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false; return true; } +#endif diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp b/extern/bullet/src/BulletCollision/Gimpact/gim_box_set.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.cpp rename to extern/bullet/src/BulletCollision/Gimpact/gim_box_set.cpp diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h b/extern/bullet/src/BulletCollision/Gimpact/gim_box_set.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_box_set.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_box_set.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h b/extern/bullet/src/BulletCollision/Gimpact/gim_clip_polygon.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_clip_polygon.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_clip_polygon.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.cpp b/extern/bullet/src/BulletCollision/Gimpact/gim_contact.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_contact.cpp rename to extern/bullet/src/BulletCollision/Gimpact/gim_contact.cpp diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h b/extern/bullet/src/BulletCollision/Gimpact/gim_contact.h similarity index 97% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_contact.h index 5d9f8ef81807..b41c714b5f72 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_contact.h +++ b/extern/bullet/src/BulletCollision/Gimpact/gim_contact.h @@ -40,8 +40,15 @@ email: projectileman@yahoo.com /** Configuration var for applying interpolation of contact normals */ +#ifndef NORMAL_CONTACT_AVERAGE #define NORMAL_CONTACT_AVERAGE 1 +#endif + +#ifndef CONTACT_DIFF_EPSILON #define CONTACT_DIFF_EPSILON 0.00001f +#endif + +#ifndef BT_CONTACT_H_STRUCTS_INCLUDED /// Structure for collision results ///Functions for managing and sorting contacts resulting from a collision query. @@ -121,6 +128,7 @@ class GIM_CONTACT }; +#endif class gim_contact_array:public gim_array { diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h b/extern/bullet/src/BulletCollision/Gimpact/gim_geom_types.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_geom_types.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_geom_types.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h b/extern/bullet/src/BulletCollision/Gimpact/gim_geometry.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_geometry.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_geometry.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h b/extern/bullet/src/BulletCollision/Gimpact/gim_hash_table.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_hash_table.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_hash_table.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h b/extern/bullet/src/BulletCollision/Gimpact/gim_linear_math.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_linear_math.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_linear_math.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_math.h b/extern/bullet/src/BulletCollision/Gimpact/gim_math.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_math.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_math.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.cpp b/extern/bullet/src/BulletCollision/Gimpact/gim_memory.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_memory.cpp rename to extern/bullet/src/BulletCollision/Gimpact/gim_memory.cpp diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h b/extern/bullet/src/BulletCollision/Gimpact/gim_memory.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_memory.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_memory.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h b/extern/bullet/src/BulletCollision/Gimpact/gim_radixsort.h similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_radixsort.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_radixsort.h diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp b/extern/bullet/src/BulletCollision/Gimpact/gim_tri_collision.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp rename to extern/bullet/src/BulletCollision/Gimpact/gim_tri_collision.cpp diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h b/extern/bullet/src/BulletCollision/Gimpact/gim_tri_collision.h similarity index 99% rename from extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h rename to extern/bullet/src/BulletCollision/Gimpact/gim_tri_collision.h index 5b552a1ed517..267f806e7e53 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h +++ b/extern/bullet/src/BulletCollision/Gimpact/gim_tri_collision.h @@ -38,8 +38,9 @@ email: projectileman@yahoo.com - +#ifndef MAX_TRI_CLIPPING #define MAX_TRI_CLIPPING 16 +#endif //! Structure for collision struct GIM_TRIANGLE_CONTACT_DATA diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h similarity index 97% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h index 9eb880b8dfca..bb8d8b872d15 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btComputeGjkEpaPenetration.h @@ -1,369 +1,369 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2014 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. -*/ - -#ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H -#define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H - -#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision... -#include "btGjkEpa3.h" -#include "btGjkCollisionDescription.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" - - - - - - -template -bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b, - const btGjkCollisionDescription& colDesc, - btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB) -{ - (void)v; - - // const btScalar radialmargin(btScalar(0.)); - - btVector3 guessVector(b.getWorldTransform().getOrigin()-a.getWorldTransform().getOrigin());//?? why not use the GJK input? - - btGjkEpaSolver3::sResults results; - - - if(btGjkEpaSolver3_Penetration(a,b,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(btGjkEpaSolver3_Distance(a,b,guessVector,results)) - { - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - v = results.normal; - return false; - } - } - return false; -} - -template -int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo) -{ - - bool m_catchDegeneracies = true; - btScalar m_cachedSeparatingDistance = 0.f; - - btScalar distance=btScalar(0.); - btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); - - btVector3 pointOnA,pointOnB; - btTransform localTransA = a.getWorldTransform(); - btTransform localTransB = b.getWorldTransform(); - - btScalar marginA = a.getMargin(); - btScalar marginB = b.getMargin(); - - int m_curIter = 0; - int gGjkMaxIter = colDesc.m_maxGjkIterations;//this is to catch invalid input, perhaps check for #NaN? - btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir; - - bool isValid = false; - bool checkSimplex = false; - bool checkPenetration = true; - int m_degenerateSimplex = 0; - - int m_lastUsedMethod = -1; - - { - btScalar squaredDistance = BT_LARGE_FLOAT; - btScalar delta = btScalar(0.); - - btScalar margin = marginA + marginB; - - - - simplexSolver.reset(); - - for ( ; ; ) - //while (true) - { - - btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis(); - btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis(); - - btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA); - btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - - - - btVector3 w = pWorld - qWorld; - delta = m_cachedSeparatingAxis.dot(w); - - // potential exit, they don't overlap - if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.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 (simplexSolver.inSimplex(w)) - { - m_degenerateSimplex = 1; - checkSimplex = true; - break; - } - // are we getting any closer ? - btScalar f0 = squaredDistance - delta; - btScalar f1 = squaredDistance * colDesc.m_gjkRelError2; - - if (f0 <= f1) - { - if (f0 <= btScalar(0.)) - { - m_degenerateSimplex = 2; - } else - { - m_degenerateSimplex = 11; - } - checkSimplex = true; - break; - } - - //add current vertex to simplex - simplexSolver.addVertex(w, pWorld, qWorld); - btVector3 newCachedSeparatingAxis; - - //calculate the closest point to the origin (update vector v) - if (!simplexSolver.closest(newCachedSeparatingAxis)) - { - m_degenerateSimplex = 3; - checkSimplex = true; - break; - } - - if(newCachedSeparatingAxis.length2()previousSquaredDistance) - { - m_degenerateSimplex = 7; - squaredDistance = previousSquaredDistance; - checkSimplex = false; - break; - } -#endif // - - - //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; - } - - m_cachedSeparatingAxis = newCachedSeparatingAxis; - - //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject - if (m_curIter++ > gGjkMaxIter) - { -#if defined(DEBUG) || defined (_DEBUG) - - printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); - printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n", - m_cachedSeparatingAxis.getX(), - m_cachedSeparatingAxis.getY(), - m_cachedSeparatingAxis.getZ(), - squaredDistance); -#endif - - break; - - } - - - bool check = (!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) - { - simplexSolver.compute_points(pointOnA, pointOnB); - normalInB = m_cachedSeparatingAxis; - - 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_degenerateSimplex && ((distance+margin) < 0.01)); - - //if (checkPenetration && !isValid) - if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) - { - //penetration case - - //if there is no way to handle penetrations, bail out - - // Penetration depth case. - btVector3 tmpPointOnA,tmpPointOnB; - - m_cachedSeparatingAxis.setZero(); - - bool isValid2 = btGjkEpaCalcPenDepth(a,b, - colDesc, - m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB); - - 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 < colDesc.m_maximumDistanceSquared))) - { - - m_cachedSeparatingAxis = normalInB; - m_cachedSeparatingDistance = distance; - distInfo->m_distance = distance; - distInfo->m_normalBtoA = normalInB; - distInfo->m_pointOnB = pointOnB; - distInfo->m_pointOnA = pointOnB+normalInB*distance; - return 0; - } - return -m_lastUsedMethod; -} - - - - -#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2014 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. +*/ + +#ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H +#define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H + +#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision... +#include "btGjkEpa3.h" +#include "btGjkCollisionDescription.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" + + + + + + +template +bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b, + const btGjkCollisionDescription& colDesc, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB) +{ + (void)v; + + // const btScalar radialmargin(btScalar(0.)); + + btVector3 guessVector(b.getWorldTransform().getOrigin()-a.getWorldTransform().getOrigin());//?? why not use the GJK input? + + btGjkEpaSolver3::sResults results; + + + if(btGjkEpaSolver3_Penetration(a,b,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(btGjkEpaSolver3_Distance(a,b,guessVector,results)) + { + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return false; + } + } + return false; +} + +template +int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo) +{ + + bool m_catchDegeneracies = true; + btScalar m_cachedSeparatingDistance = 0.f; + + btScalar distance=btScalar(0.); + btVector3 normalInB(btScalar(0.),btScalar(0.),btScalar(0.)); + + btVector3 pointOnA,pointOnB; + btTransform localTransA = a.getWorldTransform(); + btTransform localTransB = b.getWorldTransform(); + + btScalar marginA = a.getMargin(); + btScalar marginB = b.getMargin(); + + int m_curIter = 0; + int gGjkMaxIter = colDesc.m_maxGjkIterations;//this is to catch invalid input, perhaps check for #NaN? + btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir; + + bool isValid = false; + bool checkSimplex = false; + bool checkPenetration = true; + int m_degenerateSimplex = 0; + + int m_lastUsedMethod = -1; + + { + btScalar squaredDistance = BT_LARGE_FLOAT; + btScalar delta = btScalar(0.); + + btScalar margin = marginA + marginB; + + + + simplexSolver.reset(); + + for ( ; ; ) + //while (true) + { + + btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis(); + btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis(); + + btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA); + btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB); + + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + + + + btVector3 w = pWorld - qWorld; + delta = m_cachedSeparatingAxis.dot(w); + + // potential exit, they don't overlap + if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.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 (simplexSolver.inSimplex(w)) + { + m_degenerateSimplex = 1; + checkSimplex = true; + break; + } + // are we getting any closer ? + btScalar f0 = squaredDistance - delta; + btScalar f1 = squaredDistance * colDesc.m_gjkRelError2; + + if (f0 <= f1) + { + if (f0 <= btScalar(0.)) + { + m_degenerateSimplex = 2; + } else + { + m_degenerateSimplex = 11; + } + checkSimplex = true; + break; + } + + //add current vertex to simplex + simplexSolver.addVertex(w, pWorld, qWorld); + btVector3 newCachedSeparatingAxis; + + //calculate the closest point to the origin (update vector v) + if (!simplexSolver.closest(newCachedSeparatingAxis)) + { + m_degenerateSimplex = 3; + checkSimplex = true; + break; + } + + if(newCachedSeparatingAxis.length2()previousSquaredDistance) + { + m_degenerateSimplex = 7; + squaredDistance = previousSquaredDistance; + checkSimplex = false; + break; + } +#endif // + + + //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; + } + + m_cachedSeparatingAxis = newCachedSeparatingAxis; + + //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject + if (m_curIter++ > gGjkMaxIter) + { +#if defined(DEBUG) || defined (_DEBUG) + + printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); + printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n", + m_cachedSeparatingAxis.getX(), + m_cachedSeparatingAxis.getY(), + m_cachedSeparatingAxis.getZ(), + squaredDistance); +#endif + + break; + + } + + + bool check = (!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) + { + simplexSolver.compute_points(pointOnA, pointOnB); + normalInB = m_cachedSeparatingAxis; + + 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_degenerateSimplex && ((distance+margin) < 0.01)); + + //if (checkPenetration && !isValid) + if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) + { + //penetration case + + //if there is no way to handle penetrations, bail out + + // Penetration depth case. + btVector3 tmpPointOnA,tmpPointOnB; + + m_cachedSeparatingAxis.setZero(); + + bool isValid2 = btGjkEpaCalcPenDepth(a,b, + colDesc, + m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB); + + 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 < colDesc.m_maximumDistanceSquared))) + { + + m_cachedSeparatingAxis = normalInB; + m_cachedSeparatingDistance = distance; + distInfo->m_distance = distance; + distInfo->m_normalBtoA = normalInB; + distInfo->m_pointOnB = pointOnB; + distInfo->m_pointOnA = pointOnB+normalInB*distance; + return 0; + } + return -m_lastUsedMethod; +} + + + + +#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp similarity index 97% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index 940282f5762a..3481fec85060 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -113,12 +113,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f) return false; - - btScalar lambda = btScalar(0.); - btVector3 v(1,0,0); - - int maxIter = MAX_ITERATIONS; btVector3 n; n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); @@ -137,8 +132,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( btPointCollector pointCollector1; - { - + { computeClosestPoints(fromA,fromB,pointCollector1); hasResult = pointCollector1.m_hasResult; @@ -172,28 +166,20 @@ bool btContinuousConvexCollision::calcTimeOfImpact( dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); - - - lambda = lambda + dLambda; + lambda += dLambda; - if (lambda > btScalar(1.)) + if (lambda > btScalar(1.) || lambda < btScalar(0.)) 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; + //break; } lastLambda = lambda; - - //interpolate to next lambda btTransform interpolatedTransA,interpolatedTransB,relativeTrans; @@ -223,7 +209,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( } numIter++; - if (numIter > maxIter) + if (numIter > MAX_ITERATIONS) { result.reportFailure(-2, numIter); return false; @@ -237,6 +223,5 @@ bool btContinuousConvexCollision::calcTimeOfImpact( } return false; - } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h similarity index 96% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h index bdc0572f75af..528b5e010143 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h @@ -25,7 +25,7 @@ class btStaticPlaneShape; /// btContinuousConvexCollision implements angular and linear time of impact for convex objects. /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis). -/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent. +/// Algorithm operates in worldspace, in order to keep in between motion globally consistent. /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops class btContinuousConvexCollision : public btConvexCast { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h similarity index 99% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index 46ce1ab75e75..0ea7b483cfba 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -67,10 +67,12 @@ struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result btVector3 m_closestPointInB; btScalar m_distance; //negative means penetration ! + protected: btStorageResult() : m_distance(btScalar(BT_LARGE_FLOAT)) { - } + + public: virtual ~btStorageResult() {}; virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkCollisionDescription.h diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp similarity index 96% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index 3268f06c2f92..eefb974bbd2c 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -41,21 +41,38 @@ namespace gjkepa2_impl /* 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) + +#ifdef BT_USE_DOUBLE_PRECISION + #define GJK_ACCURACY ((btScalar)1e-12) + #define GJK_MIN_DISTANCE ((btScalar)1e-12) + #define GJK_DUPLICATED_EPS ((btScalar)1e-12) +#else + #define GJK_ACCURACY ((btScalar)0.0001) + #define GJK_MIN_DISTANCE ((btScalar)0.0001) + #define GJK_DUPLICATED_EPS ((btScalar)0.0001) +#endif //BT_USE_DOUBLE_PRECISION + + #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_VERTICES 128 #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) + +#ifdef BT_USE_DOUBLE_PRECISION + #define EPA_ACCURACY ((btScalar)1e-12) + #define EPA_PLANE_EPS ((btScalar)1e-14) + #define EPA_INSIDE_EPS ((btScalar)1e-9) +#else + #define EPA_ACCURACY ((btScalar)0.0001) + #define EPA_PLANE_EPS ((btScalar)0.00001) + #define EPA_INSIDE_EPS ((btScalar)0.01) +#endif + +#define EPA_FALLBACK (10*EPA_ACCURACY) +#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) // Shorthands @@ -242,7 +259,7 @@ namespace gjkepa2_impl /* Check for termination */ const btScalar omega=btDot(m_ray,w)/rl; alpha=btMax(omega,alpha); - if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) + if(((rl-alpha)-(GJK_ACCURACY*rl))<=0) {/* Return old simplex */ removevertice(m_simplices[m_current]); break; @@ -1015,7 +1032,7 @@ bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, /* Symbols cleanup */ #undef GJK_MAX_ITERATIONS -#undef GJK_ACCURARY +#undef GJK_ACCURACY #undef GJK_MIN_DISTANCE #undef GJK_DUPLICATED_EPS #undef GJK_SIMPLEX2_EPS diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h similarity index 97% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h index ce1f24bc50db..878949af940b 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpa3.h @@ -1,1035 +1,1035 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2014 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. -*/ - -/* -Initial GJK-EPA collision solver by Nathanael Presson, 2008 -Improvements and refactoring by Erwin Coumans, 2008-2014 -*/ -#ifndef BT_GJK_EPA3_H -#define BT_GJK_EPA3_H - -#include "LinearMath/btTransform.h" -#include "btGjkCollisionDescription.h" - - - -struct btGjkEpaSolver3 -{ -struct sResults - { - enum eStatus - { - Separated, /* Shapes doesnt penetrate */ - Penetrating, /* Shapes are penetrating */ - GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ - EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ - } status; - btVector3 witnesses[2]; - btVector3 normal; - btScalar distance; - }; - - -}; - - - -#if defined(DEBUG) || defined (_DEBUG) -#include //for debug printf -#ifdef __SPU__ -#include -#define printf spu_printf -#endif //__SPU__ -#endif - - - - // 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 - template - struct MinkowskiDiff - { - const btConvexTemplate* m_convexAPtr; - const btConvexTemplate* m_convexBPtr; - - btMatrix3x3 m_toshape1; - btTransform m_toshape0; - - bool m_enableMargin; - - - MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b) - :m_convexAPtr(&a), - m_convexBPtr(&b) - { - } - - void EnableMargin(bool enable) - { - m_enableMargin = enable; - } - inline btVector3 Support0(const btVector3& d) const - { - return m_convexAPtr->getLocalSupportWithMargin(d); - } - inline btVector3 Support1(const btVector3& d) const - { - return m_toshape0*m_convexBPtr->getLocalSupportWithMargin(m_toshape1*d); - } - - - 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)); - } - }; - -enum eGjkStatus -{ - eGjkValid, - eGjkInside, - eGjkFailed -}; - - // GJK - template - struct GJK - { - /* Types */ - struct sSV - { - btVector3 d,w; - }; - struct sSimplex - { - sSV* c[4]; - btScalar p[4]; - U rank; - }; - - /* Fields */ - - MinkowskiDiff 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; - eGjkStatus m_status; - /* Methods */ - - GJK(const btConvexTemplate& a, const btConvexTemplate& b) - :m_shape(a,b) - { - Initialize(); - } - void Initialize() - { - m_ray = btVector3(0,0,0); - m_nfree = 0; - m_status = eGjkFailed; - m_current = 0; - m_distance = 0; - } - eGjkStatus Evaluate(const MinkowskiDiff& 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 = eGjkValid; - m_shape = shapearg; - m_distance = 0; - /* Initialize simplex */ - m_simplices[0].rank = 0; - m_ray = guess; - const btScalar sqrl= m_ray.length2(); - appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); - m_simplices[0].p[0] = 1; - m_ray = m_simplices[0].c[0]->w; - sqdist = sqrl; - lastw[0] = - lastw[1] = - lastw[2] = - lastw[3] = m_ray; - /* Loop */ - do { - const U next=1-m_current; - sSimplex& cs=m_simplices[m_current]; - sSimplex& ns=m_simplices[next]; - /* Check zero */ - const btScalar rl=m_ray.length(); - if(rlw; - bool found=false; - for(U i=0;i<4;++i) - { - if((w-lastw[i]).length2()w, - cs.c[1]->w, - weights,mask);break; - case 3: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - weights,mask);break; - case 4: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - cs.c[3]->w, - weights,mask);break; - } - if(sqdist>=0) - {/* Valid */ - ns.rank = 0; - m_ray = btVector3(0,0,0); - m_current = next; - for(U i=0,ni=cs.rank;iw*weights[i]; - } - else - { - m_free[m_nfree++] = cs.c[i]; - } - } - if(mask==15) m_status=eGjkInside; - } - else - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - m_status=((++iterations)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(((subm&1)?1<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((subm&1?1< - struct EPA - { - /* Types */ - - struct sFace - { - btVector3 n; - btScalar d; - typename GJK::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) {} - }; - - /* Fields */ - eEpaStatus m_status; - typename GJK::sSimplex m_result; - btVector3 m_normal; - btScalar m_depth; - typename GJK::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 = eEpaFailed; - m_normal = btVector3(0,0,0); - m_depth = 0; - m_nextsv = 0; - for(U i=0;i& gjk,const btVector3& guess) - { - typename 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 = eEpaValid; - 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=eEpaValid; - for(;iterations::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(); - outer=*best; - } else { m_status=eEpaInvalidHull;break; } - } else { m_status=eEpaAccuraryReached;break; } - } else { m_status=eEpaOutOfVertices;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 = eEpaFallBack; - 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); - } - bool getedgedist(sFace* face, typename GJK::sSV* a, typename GJK::sSV* b, btScalar& dist) - { - const btVector3 ba = b->w - a->w; - const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane - const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required - - if(a_dot_nab < 0) - { - // Outside of edge a->b - - const btScalar ba_l2 = ba.length2(); - const btScalar a_dot_ba = btDot(a->w, ba); - const btScalar b_dot_ba = btDot(b->w, ba); - - if(a_dot_ba > 0) - { - // Pick distance vertex a - dist = a->w.length(); - } - else if(b_dot_ba < 0) - { - // Pick distance vertex b - dist = b->w.length(); - } - else - { - // Pick distance to edge a->b - const btScalar a_dot_b = btDot(a->w, b->w); - dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); - } - - return true; - } - - return false; - } - sFace* newface(typename GJK::sSV* a,typename GJK::sSV* b,typename GJK::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; - - if(v) - { - if(!(getedgedist(face, a, b, face->d) || - getedgedist(face, b, c, face->d) || - getedgedist(face, c, a, face->d))) - { - // Origin projects to the interior of the triangle - // Use distance to triangle plane - face->d = btDot(a->w, face->n) / l; - } - - face->n /= l; - if(forced || (face->d >= -EPA_PLANE_EPS)) - { - return face; - } - else - m_status=eEpaNonConvex; - } - else - m_status=eEpaDegenerated; - - remove(m_hull, face); - append(m_stock, face); - return 0; - - } - m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces; - return 0; - } - sFace* findbest() - { - sFace* minf=m_hull.root; - btScalar mind=minf->d*minf->d; - for(sFace* f=minf->l[1];f;f=f->l[1]) - { - const btScalar sqd=f->d*f->d; - if(sqd::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); - } - - }; - - template - static void Initialize( const btConvexTemplate& a, const btConvexTemplate& b, - btGjkEpaSolver3::sResults& results, - MinkowskiDiff& shape) - { - /* Results */ - results.witnesses[0] = - results.witnesses[1] = btVector3(0,0,0); - results.status = btGjkEpaSolver3::sResults::Separated; - /* Shape */ - - shape.m_toshape1 = b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis()); - shape.m_toshape0 = a.getWorldTransform().inverseTimes(b.getWorldTransform()); - - } - - -// -// Api -// - - - -// -template -bool btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b, - const btVector3& guess, - btGjkEpaSolver3::sResults& results) -{ - MinkowskiDiff shape(a,b); - Initialize(a,b,results,shape); - GJK gjk(a,b); - eGjkStatus gjk_status=gjk.Evaluate(shape,guess); - if(gjk_status==eGjkValid) - { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;irank;++i) - { - const btScalar p=gjk.m_simplex->p[i]; - w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; - w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; - } - results.witnesses[0] = a.getWorldTransform()*w0; - results.witnesses[1] = a.getWorldTransform()*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==eGjkInside? - btGjkEpaSolver3::sResults::Penetrating : - btGjkEpaSolver3::sResults::GJK_Failed ; - return(false); - } -} - - -template -bool btGjkEpaSolver3_Penetration(const btConvexTemplate& a, - const btConvexTemplate& b, - const btVector3& guess, - btGjkEpaSolver3::sResults& results) -{ - MinkowskiDiff shape(a,b); - Initialize(a,b,results,shape); - GJK gjk(a,b); - eGjkStatus gjk_status=gjk.Evaluate(shape,-guess); - switch(gjk_status) - { - case eGjkInside: - { - EPA epa; - eEpaStatus epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=eEpaFailed) - { - btVector3 w0=btVector3(0,0,0); - for(U i=0;id,0)*epa.m_result.p[i]; - } - results.status = btGjkEpaSolver3::sResults::Penetrating; - results.witnesses[0] = a.getWorldTransform()*w0; - results.witnesses[1] = a.getWorldTransform()*(w0-epa.m_normal*epa.m_depth); - results.normal = -epa.m_normal; - results.distance = -epa.m_depth; - return(true); - } else results.status=btGjkEpaSolver3::sResults::EPA_Failed; - } - break; - case eGjkFailed: - results.status=btGjkEpaSolver3::sResults::GJK_Failed; - break; - default: - { - } - } - return(false); -} - -#if 0 -int btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanceInfo* distInfo) -{ - btGjkEpaSolver3::sResults results; - btVector3 guess = colDesc.m_firstDir; - - bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB, - colDesc.m_transformA,colDesc.m_transformB, - colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB, - guess, - results); - if (res) - { - if ((results.status==btGjkEpaSolver3::sResults::Penetrating) || results.status==GJK::eStatus::Inside) - { - //normal could be 'swapped' - - distInfo->m_distance = results.distance; - distInfo->m_normalBtoA = results.normal; - btVector3 tmpNormalInB = results.witnesses[1]-results.witnesses[0]; - btScalar lenSqr = tmpNormalInB.length2(); - if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) - { - tmpNormalInB = results.normal; - lenSqr = results.normal.length2(); - } - - if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) - { - tmpNormalInB /= btSqrt(lenSqr); - btScalar distance2 = -(results.witnesses[0]-results.witnesses[1]).length(); - //only replace valid penetrations when the result is deeper (check) - //if ((distance2 < results.distance)) - { - distInfo->m_distance = distance2; - distInfo->m_pointOnA= results.witnesses[0]; - distInfo->m_pointOnB= results.witnesses[1]; - distInfo->m_normalBtoA= tmpNormalInB; - return 0; - } - } - } - - } - - return -1; -} -#endif - -template -int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b, - const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo) -{ - btGjkEpaSolver3::sResults results; - btVector3 guess = colDesc.m_firstDir; - - bool isSeparated = btGjkEpaSolver3_Distance( a,b, - guess, - results); - if (isSeparated) - { - distInfo->m_distance = results.distance; - distInfo->m_pointOnA= results.witnesses[0]; - distInfo->m_pointOnB= results.witnesses[1]; - distInfo->m_normalBtoA= results.normal; - return 0; - } - - return -1; -} - -/* 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 - - - -#endif //BT_GJK_EPA3_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2014 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. +*/ + +/* +Initial GJK-EPA collision solver by Nathanael Presson, 2008 +Improvements and refactoring by Erwin Coumans, 2008-2014 +*/ +#ifndef BT_GJK_EPA3_H +#define BT_GJK_EPA3_H + +#include "LinearMath/btTransform.h" +#include "btGjkCollisionDescription.h" + + + +struct btGjkEpaSolver3 +{ +struct sResults + { + enum eStatus + { + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } status; + btVector3 witnesses[2]; + btVector3 normal; + btScalar distance; + }; + + +}; + + + +#if defined(DEBUG) || defined (_DEBUG) +#include //for debug printf +#ifdef __SPU__ +#include +#define printf spu_printf +#endif //__SPU__ +#endif + + + + // 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 + template + struct MinkowskiDiff + { + const btConvexTemplate* m_convexAPtr; + const btConvexTemplate* m_convexBPtr; + + btMatrix3x3 m_toshape1; + btTransform m_toshape0; + + bool m_enableMargin; + + + MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b) + :m_convexAPtr(&a), + m_convexBPtr(&b) + { + } + + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline btVector3 Support0(const btVector3& d) const + { + return m_convexAPtr->getLocalSupportWithMargin(d); + } + inline btVector3 Support1(const btVector3& d) const + { + return m_toshape0*m_convexBPtr->getLocalSupportWithMargin(m_toshape1*d); + } + + + 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)); + } + }; + +enum eGjkStatus +{ + eGjkValid, + eGjkInside, + eGjkFailed +}; + + // GJK + template + struct GJK + { + /* Types */ + struct sSV + { + btVector3 d,w; + }; + struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; + + /* Fields */ + + MinkowskiDiff 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; + eGjkStatus m_status; + /* Methods */ + + GJK(const btConvexTemplate& a, const btConvexTemplate& b) + :m_shape(a,b) + { + Initialize(); + } + void Initialize() + { + m_ray = btVector3(0,0,0); + m_nfree = 0; + m_status = eGjkFailed; + m_current = 0; + m_distance = 0; + } + eGjkStatus Evaluate(const MinkowskiDiff& 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 = eGjkValid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const btScalar sqrl= m_ray.length2(); + appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do { + const U next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const btScalar rl=m_ray.length(); + if(rlw; + bool found=false; + for(U i=0;i<4;++i) + { + if((w-lastw[i]).length2()w, + cs.c[1]->w, + weights,mask);break; + case 3: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights,mask);break; + case 4: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights,mask);break; + } + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = btVector3(0,0,0); + m_current = next; + for(U i=0,ni=cs.rank;iw*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) m_status=eGjkInside; + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)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(((subm&1)?1<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((subm&1?1< + struct EPA + { + /* Types */ + + struct sFace + { + btVector3 n; + btScalar d; + typename GJK::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) {} + }; + + /* Fields */ + eEpaStatus m_status; + typename GJK::sSimplex m_result; + btVector3 m_normal; + btScalar m_depth; + typename GJK::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 = eEpaFailed; + m_normal = btVector3(0,0,0); + m_depth = 0; + m_nextsv = 0; + for(U i=0;i& gjk,const btVector3& guess) + { + typename 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 = eEpaValid; + 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=eEpaValid; + for(;iterations::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(); + outer=*best; + } else { m_status=eEpaInvalidHull;break; } + } else { m_status=eEpaAccuraryReached;break; } + } else { m_status=eEpaOutOfVertices;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 = eEpaFallBack; + 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); + } + bool getedgedist(sFace* face, typename GJK::sSV* a, typename GJK::sSV* b, btScalar& dist) + { + const btVector3 ba = b->w - a->w; + const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required + + if(a_dot_nab < 0) + { + // Outside of edge a->b + + const btScalar ba_l2 = ba.length2(); + const btScalar a_dot_ba = btDot(a->w, ba); + const btScalar b_dot_ba = btDot(b->w, ba); + + if(a_dot_ba > 0) + { + // Pick distance vertex a + dist = a->w.length(); + } + else if(b_dot_ba < 0) + { + // Pick distance vertex b + dist = b->w.length(); + } + else + { + // Pick distance to edge a->b + const btScalar a_dot_b = btDot(a->w, b->w); + dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); + } + + return true; + } + + return false; + } + sFace* newface(typename GJK::sSV* a,typename GJK::sSV* b,typename GJK::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; + + if(v) + { + if(!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) + { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = btDot(a->w, face->n) / l; + } + + face->n /= l; + if(forced || (face->d >= -EPA_PLANE_EPS)) + { + return face; + } + else + m_status=eEpaNonConvex; + } + else + m_status=eEpaDegenerated; + + remove(m_hull, face); + append(m_stock, face); + return 0; + + } + m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces; + return 0; + } + sFace* findbest() + { + sFace* minf=m_hull.root; + btScalar mind=minf->d*minf->d; + for(sFace* f=minf->l[1];f;f=f->l[1]) + { + const btScalar sqd=f->d*f->d; + if(sqd::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); + } + + }; + + template + static void Initialize( const btConvexTemplate& a, const btConvexTemplate& b, + btGjkEpaSolver3::sResults& results, + MinkowskiDiff& shape) + { + /* Results */ + results.witnesses[0] = + results.witnesses[1] = btVector3(0,0,0); + results.status = btGjkEpaSolver3::sResults::Separated; + /* Shape */ + + shape.m_toshape1 = b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis()); + shape.m_toshape0 = a.getWorldTransform().inverseTimes(b.getWorldTransform()); + + } + + +// +// Api +// + + + +// +template +bool btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b, + const btVector3& guess, + btGjkEpaSolver3::sResults& results) +{ + MinkowskiDiff shape(a,b); + Initialize(a,b,results,shape); + GJK gjk(a,b); + eGjkStatus gjk_status=gjk.Evaluate(shape,guess); + if(gjk_status==eGjkValid) + { + btVector3 w0=btVector3(0,0,0); + btVector3 w1=btVector3(0,0,0); + for(U i=0;irank;++i) + { + const btScalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = a.getWorldTransform()*w0; + results.witnesses[1] = a.getWorldTransform()*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==eGjkInside? + btGjkEpaSolver3::sResults::Penetrating : + btGjkEpaSolver3::sResults::GJK_Failed ; + return(false); + } +} + + +template +bool btGjkEpaSolver3_Penetration(const btConvexTemplate& a, + const btConvexTemplate& b, + const btVector3& guess, + btGjkEpaSolver3::sResults& results) +{ + MinkowskiDiff shape(a,b); + Initialize(a,b,results,shape); + GJK gjk(a,b); + eGjkStatus gjk_status=gjk.Evaluate(shape,-guess); + switch(gjk_status) + { + case eGjkInside: + { + EPA epa; + eEpaStatus epa_status=epa.Evaluate(gjk,-guess); + if(epa_status!=eEpaFailed) + { + btVector3 w0=btVector3(0,0,0); + for(U i=0;id,0)*epa.m_result.p[i]; + } + results.status = btGjkEpaSolver3::sResults::Penetrating; + results.witnesses[0] = a.getWorldTransform()*w0; + results.witnesses[1] = a.getWorldTransform()*(w0-epa.m_normal*epa.m_depth); + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return(true); + } else results.status=btGjkEpaSolver3::sResults::EPA_Failed; + } + break; + case eGjkFailed: + results.status=btGjkEpaSolver3::sResults::GJK_Failed; + break; + default: + { + } + } + return(false); +} + +#if 0 +int btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanceInfo* distInfo) +{ + btGjkEpaSolver3::sResults results; + btVector3 guess = colDesc.m_firstDir; + + bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB, + colDesc.m_transformA,colDesc.m_transformB, + colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB, + guess, + results); + if (res) + { + if ((results.status==btGjkEpaSolver3::sResults::Penetrating) || results.status==GJK::eStatus::Inside) + { + //normal could be 'swapped' + + distInfo->m_distance = results.distance; + distInfo->m_normalBtoA = results.normal; + btVector3 tmpNormalInB = results.witnesses[1]-results.witnesses[0]; + btScalar lenSqr = tmpNormalInB.length2(); + if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) + { + tmpNormalInB = results.normal; + lenSqr = results.normal.length2(); + } + + if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) + { + tmpNormalInB /= btSqrt(lenSqr); + btScalar distance2 = -(results.witnesses[0]-results.witnesses[1]).length(); + //only replace valid penetrations when the result is deeper (check) + //if ((distance2 < results.distance)) + { + distInfo->m_distance = distance2; + distInfo->m_pointOnA= results.witnesses[0]; + distInfo->m_pointOnB= results.witnesses[1]; + distInfo->m_normalBtoA= tmpNormalInB; + return 0; + } + } + } + + } + + return -1; +} +#endif + +template +int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b, + const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo) +{ + btGjkEpaSolver3::sResults results; + btVector3 guess = colDesc.m_firstDir; + + bool isSeparated = btGjkEpaSolver3_Distance( a,b, + guess, + results); + if (isSeparated) + { + distInfo->m_distance = results.distance; + distInfo->m_pointOnA= results.witnesses[0]; + distInfo->m_pointOnB= results.witnesses[1]; + distInfo->m_normalBtoA= results.normal; + return 0; + } + + return -1; +} + +/* 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 + + + +#endif //BT_GJK_EPA3_H + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp similarity index 50% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 572ec36f5639..2fca19292a64 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -21,46 +21,64 @@ subject to the following restrictions: #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) +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) { (void)debugDraw; (void)v; (void)simplexSolver; -// const btScalar radialmargin(btScalar(0.)); - - btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin()); - btGjkEpaSolver2::sResults results; - + btVector3 guessVectors[] = { + btVector3(transformB.getOrigin() - transformA.getOrigin()), + btVector3(transformA.getOrigin() - transformB.getOrigin()), + btVector3(0, 0, 1), + btVector3(0, 1, 0), + btVector3(1, 0, 0), + btVector3(1, 1, 0), + btVector3(1, 1, 1), + btVector3(0, 1, 1), + btVector3(1, 0, 1), + }; - 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 + int numVectors = sizeof(guessVectors) / sizeof(btVector3); + + for (int i = 0; i < numVectors; i++) { - if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results)) + simplexSolver.reset(); + btVector3 guessVector = guessVectors[i]; + + btGjkEpaSolver2::sResults results; + + if (btGjkEpaSolver2::Penetration(pConvexA, transformA, + pConvexB, transformB, + guessVector, results)) + { wWitnessOnA = results.witnesses[0]; wWitnessOnB = results.witnesses[1]; v = results.normal; - return false; + 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; + } } } + //failed to find a distance/penetration + wWitnessOnA.setValue(0, 0, 0); + wWitnessOnB.setValue(0, 0, 0); + v.setValue(0, 0, 0); return false; } - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp similarity index 87% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 759443a9613f..4d29089a4a08 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -30,7 +30,13 @@ subject to the following restrictions: #endif //must be above the machine epsilon -#define REL_ERROR2 btScalar(1.0e-6) +#ifdef BT_USE_DOUBLE_PRECISION + #define REL_ERROR2 btScalar(1.0e-12) + btScalar gGjkEpaPenetrationTolerance = 1.0e-12; +#else + #define REL_ERROR2 btScalar(1.0e-6) + btScalar gGjkEpaPenetrationTolerance = 0.001; +#endif //temp globals, to improve GJK/EPA/penetration calculations int gNumDeepPenetrationChecks = 0; @@ -275,7 +281,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu btScalar lenSqr =m_cachedSeparatingAxis.length2(); //valid normal - if (lenSqr < 0.0001) + if (lenSqr < REL_ERROR2) { m_degenerateSimplex = 5; } @@ -300,7 +306,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu } bool catchDegeneratePenetrationCase = - (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01)); + (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance)); //if (checkPenetration && !isValid) if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) @@ -347,46 +353,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu pointOnA = tmpPointOnA; pointOnB = tmpPointOnB; normalInB = tmpNormalInB; - ///todo: need to track down this EPA penetration solver degeneracy - ///the penetration solver reports penetration but the contact normal - ///connecting the contact points is pointing in the opposite direction - ///until then, detect the issue and revert the normal - { - btScalar d1=0; - { - btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis(); - - - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - btVector3 w = pWorld - qWorld; - d1 = (-normalInB).dot(w); - } - btScalar d0 = 0.f; - { - btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis(); - - - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); - - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); - btVector3 w = pWorld - qWorld; - d0 = normalInB.dot(w); - } - if (d1>d0) - { - m_lastUsedMethod = 10; - normalInB*=-1; - } - - } + isValid = true; } else @@ -443,6 +410,47 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu m_cachedSeparatingAxis = normalInB; m_cachedSeparatingDistance = distance; + { + ///todo: need to track down this EPA penetration solver degeneracy + ///the penetration solver reports penetration but the contact normal + ///connecting the contact points is pointing in the opposite direction + ///until then, detect the issue and revert the normal + + btScalar d1=0; + { + btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis(); + btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis(); + + + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); + + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + btVector3 w = pWorld - qWorld; + d1 = (-normalInB).dot(w); + } + btScalar d0 = 0.f; + { + btVector3 seperatingAxisInA = (-normalInB)* input.m_transformA.getBasis(); + btVector3 seperatingAxisInB = normalInB* input.m_transformB.getBasis(); + + + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); + + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + btVector3 w = pWorld - qWorld; + d0 = normalInB.dot(w); + } + if (d1>d0) + { + m_lastUsedMethod = 10; + normalInB*=-1; + } + + } output.addContactPoint( normalInB, pointOnB+positionOffset, diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h similarity index 76% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index e40fb1d3db60..571ad2c5f72e 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -35,7 +35,14 @@ typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow; typedef btConstraintRow PfxConstraintRow; #endif //PFX_USE_FREE_VECTORMATH - +enum btContactPointFlags +{ + BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED=1, + BT_CONTACT_FLAG_HAS_CONTACT_CFM=2, + BT_CONTACT_FLAG_HAS_CONTACT_ERP=4, + BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8, + BT_CONTACT_FLAG_FRICTION_ANCHOR = 16, +}; /// ManifoldContactPoint collects and maintains persistent contactpoints. /// used to improve stability and performance of rigidbody dynamics response. @@ -44,14 +51,15 @@ class btManifoldPoint public: btManifoldPoint() :m_userPersistentData(0), - m_lateralFrictionInitialized(false), - m_appliedImpulse(0.f), + m_contactPointFlags(0), + m_appliedImpulse(0.f), m_appliedImpulseLateral1(0.f), m_appliedImpulseLateral2(0.f), m_contactMotion1(0.f), m_contactMotion2(0.f), - m_contactCFM1(0.f), - m_contactCFM2(0.f), + m_contactCFM(0.f), + m_contactERP(0.f), + m_frictionCFM(0.f), m_lifeTime(0) { } @@ -65,16 +73,18 @@ class btManifoldPoint m_distance1( distance ), m_combinedFriction(btScalar(0.)), m_combinedRollingFriction(btScalar(0.)), - m_combinedRestitution(btScalar(0.)), + m_combinedSpinningFriction(btScalar(0.)), + m_combinedRestitution(btScalar(0.)), m_userPersistentData(0), - m_lateralFrictionInitialized(false), - m_appliedImpulse(0.f), + m_contactPointFlags(0), + m_appliedImpulse(0.f), m_appliedImpulseLateral1(0.f), m_appliedImpulseLateral2(0.f), m_contactMotion1(0.f), m_contactMotion2(0.f), - m_contactCFM1(0.f), - m_contactCFM2(0.f), + m_contactCFM(0.f), + m_contactERP(0.f), + m_frictionCFM(0.f), m_lifeTime(0) { @@ -91,8 +101,9 @@ class btManifoldPoint btScalar m_distance1; btScalar m_combinedFriction; - btScalar m_combinedRollingFriction; - btScalar m_combinedRestitution; + btScalar m_combinedRollingFriction;//torsional friction orthogonal to contact normal, useful to make spheres stop rolling forever + btScalar m_combinedSpinningFriction;//torsional friction around contact normal, useful for grasping objects + btScalar m_combinedRestitution; //BP mod, store contact triangles. int m_partId0; @@ -101,15 +112,28 @@ class btManifoldPoint int m_index1; mutable void* m_userPersistentData; - bool m_lateralFrictionInitialized; - + //bool m_lateralFrictionInitialized; + int m_contactPointFlags; + btScalar m_appliedImpulse; btScalar m_appliedImpulseLateral1; btScalar m_appliedImpulseLateral2; btScalar m_contactMotion1; btScalar m_contactMotion2; - btScalar m_contactCFM1; - btScalar m_contactCFM2; + + union + { + btScalar m_contactCFM; + btScalar m_combinedContactStiffness1; + }; + + union + { + btScalar m_contactERP; + btScalar m_combinedContactDamping1; + }; + + btScalar m_frictionCFM; int m_lifeTime;//lifetime of the contactpoint in frames diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h similarity index 96% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h index a22a0bae66be..052f48b8c574 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h @@ -1,908 +1,908 @@ - -/*** - * --------------------------------- - * Copyright (c)2012 Daniel Fiser - * - * This file was ported from mpr.c file, part of libccd. - * The Minkoski Portal Refinement implementation was ported - * to OpenCL by Erwin Coumans for the Bullet 3 Physics library. - * The original MPR idea and implementation is by Gary Snethen - * in XenoCollide, see http://github.com/erwincoumans/xenocollide - * - * Distributed under the OSI-approved BSD License (the "License"); - * see . - * This software is distributed WITHOUT ANY WARRANTY; without even the - * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the License for more information. - */ - -///2014 Oct, Erwin Coumans, Use templates to avoid void* casts - -#ifndef BT_MPR_PENETRATION_H -#define BT_MPR_PENETRATION_H - -#define BT_DEBUG_MPR1 - -#include "LinearMath/btTransform.h" -#include "LinearMath/btAlignedObjectArray.h" - -//#define MPR_AVERAGE_CONTACT_POSITIONS - - -struct btMprCollisionDescription -{ - btVector3 m_firstDir; - int m_maxGjkIterations; - btScalar m_maximumDistanceSquared; - btScalar m_gjkRelError2; - - btMprCollisionDescription() - : m_firstDir(0,1,0), - m_maxGjkIterations(1000), - m_maximumDistanceSquared(1e30f), - m_gjkRelError2(1.0e-6) - { - } - virtual ~btMprCollisionDescription() - { - } -}; - -struct btMprDistanceInfo -{ - btVector3 m_pointOnA; - btVector3 m_pointOnB; - btVector3 m_normalBtoA; - btScalar m_distance; -}; - -#ifdef __cplusplus -#define BT_MPR_SQRT sqrtf -#else -#define BT_MPR_SQRT sqrt -#endif -#define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y)) -#define BT_MPR_FABS fabs - -#define BT_MPR_TOLERANCE 1E-6f -#define BT_MPR_MAX_ITERATIONS 1000 - -struct _btMprSupport_t -{ - btVector3 v; //!< Support point in minkowski sum - btVector3 v1; //!< Support point in obj1 - btVector3 v2; //!< Support point in obj2 -}; -typedef struct _btMprSupport_t btMprSupport_t; - -struct _btMprSimplex_t -{ - btMprSupport_t ps[4]; - int last; //!< index of last added point -}; -typedef struct _btMprSimplex_t btMprSimplex_t; - -inline btMprSupport_t* btMprSimplexPointW(btMprSimplex_t *s, int idx) -{ - return &s->ps[idx]; -} - -inline void btMprSimplexSetSize(btMprSimplex_t *s, int size) -{ - s->last = size - 1; -} - -#ifdef DEBUG_MPR -inline void btPrintPortalVertex(_btMprSimplex_t* portal, int index) -{ - printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(),portal->ps[index].v.y(),portal->ps[index].v.z(), - portal->ps[index].v1.x(),portal->ps[index].v1.y(),portal->ps[index].v1.z(), - portal->ps[index].v2.x(),portal->ps[index].v2.y(),portal->ps[index].v2.z()); -} -#endif //DEBUG_MPR - - - - -inline int btMprSimplexSize(const btMprSimplex_t *s) -{ - return s->last + 1; -} - - -inline const btMprSupport_t* btMprSimplexPoint(const btMprSimplex_t* s, int idx) -{ - // here is no check on boundaries - return &s->ps[idx]; -} - -inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s) -{ - *d = *s; -} - -inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a) -{ - btMprSupportCopy(s->ps + pos, a); -} - - -inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2) -{ - btMprSupport_t supp; - - btMprSupportCopy(&supp, &s->ps[pos1]); - btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]); - btMprSupportCopy(&s->ps[pos2], &supp); -} - - -inline int btMprIsZero(float val) -{ - return BT_MPR_FABS(val) < FLT_EPSILON; -} - - - -inline int btMprEq(float _a, float _b) -{ - float ab; - float a, b; - - ab = BT_MPR_FABS(_a - _b); - if (BT_MPR_FABS(ab) < FLT_EPSILON) - return 1; - - a = BT_MPR_FABS(_a); - b = BT_MPR_FABS(_b); - if (b > a){ - return ab < FLT_EPSILON * b; - }else{ - return ab < FLT_EPSILON * a; - } -} - - -inline int btMprVec3Eq(const btVector3* a, const btVector3 *b) -{ - return btMprEq((*a).x(), (*b).x()) - && btMprEq((*a).y(), (*b).y()) - && btMprEq((*a).z(), (*b).z()); -} - - - - - - - - - - - -template -inline void btFindOrigin(const btConvexTemplate& a, const btConvexTemplate& b, const btMprCollisionDescription& colDesc,btMprSupport_t *center) -{ - - center->v1 = a.getObjectCenterInWorld(); - center->v2 = b.getObjectCenterInWorld(); - center->v = center->v1 - center->v2; -} - -inline void btMprVec3Set(btVector3 *v, float x, float y, float z) -{ - v->setValue(x,y,z); -} - -inline void btMprVec3Add(btVector3 *v, const btVector3 *w) -{ - *v += *w; -} - -inline void btMprVec3Copy(btVector3 *v, const btVector3 *w) -{ - *v = *w; -} - -inline void btMprVec3Scale(btVector3 *d, float k) -{ - *d *= k; -} - -inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b) -{ - float dot; - - dot = btDot(*a,*b); - return dot; -} - - -inline float btMprVec3Len2(const btVector3 *v) -{ - return btMprVec3Dot(v, v); -} - -inline void btMprVec3Normalize(btVector3 *d) -{ - float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d)); - btMprVec3Scale(d, k); -} - -inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b) -{ - *d = btCross(*a,*b); - -} - - -inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) -{ - *d = *v - *w; -} - -inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir) -{ - btVector3 v2v1, v3v1; - - btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 1)->v); - btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v, - &btMprSimplexPoint(portal, 1)->v); - btMprVec3Cross(dir, &v2v1, &v3v1); - btMprVec3Normalize(dir); -} - - -inline int portalEncapsulesOrigin(const btMprSimplex_t *portal, - const btVector3 *dir) -{ - float dot; - dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v); - return btMprIsZero(dot) || dot > 0.f; -} - -inline int portalReachTolerance(const btMprSimplex_t *portal, - const btMprSupport_t *v4, - const btVector3 *dir) -{ - float dv1, dv2, dv3, dv4; - float dot1, dot2, dot3; - - // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} - - dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir); - dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir); - dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir); - dv4 = btMprVec3Dot(&v4->v, dir); - - dot1 = dv4 - dv1; - dot2 = dv4 - dv2; - dot3 = dv4 - dv3; - - dot1 = BT_MPR_FMIN(dot1, dot2); - dot1 = BT_MPR_FMIN(dot1, dot3); - - return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE; -} - -inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal, - const btMprSupport_t *v4, - const btVector3 *dir) -{ - float dot; - dot = btMprVec3Dot(&v4->v, dir); - return btMprIsZero(dot) || dot > 0.f; -} - -inline void btExpandPortal(btMprSimplex_t *portal, - const btMprSupport_t *v4) -{ - float dot; - btVector3 v4v0; - - btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v); - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0); - if (dot > 0.f){ - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0); - if (dot > 0.f){ - btMprSimplexSet(portal, 1, v4); - }else{ - btMprSimplexSet(portal, 3, v4); - } - }else{ - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0); - if (dot > 0.f){ - btMprSimplexSet(portal, 2, v4); - }else{ - btMprSimplexSet(portal, 1, v4); - } - } -} -template -inline void btMprSupport(const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - const btVector3& dir, btMprSupport_t *supp) -{ - btVector3 seperatingAxisInA = dir* a.getWorldTransform().getBasis(); - btVector3 seperatingAxisInB = -dir* b.getWorldTransform().getBasis(); - - btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA); - btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB); - - supp->v1 = a.getWorldTransform()(pInA); - supp->v2 = b.getWorldTransform()(qInB); - supp->v = supp->v1 - supp->v2; -} - - -template -static int btDiscoverPortal(const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - btMprSimplex_t *portal) -{ - btVector3 dir, va, vb; - float dot; - int cont; - - - - // vertex 0 is center of portal - btFindOrigin(a,b,colDesc, btMprSimplexPointW(portal, 0)); - - - // vertex 0 is center of portal - btMprSimplexSetSize(portal, 1); - - - - btVector3 zero = btVector3(0,0,0); - btVector3* org = &zero; - - if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org)){ - // Portal's center lies on origin (0,0,0) => we know that objects - // intersect but we would need to know penetration info. - // So move center little bit... - btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f); - btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va); - } - - - // vertex 1 = support in direction of origin - btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v); - btMprVec3Scale(&dir, -1.f); - btMprVec3Normalize(&dir); - - - btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 1)); - - btMprSimplexSetSize(portal, 2); - - // test if origin isn't outside of v1 - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir); - - - if (btMprIsZero(dot) || dot < 0.f) - return -1; - - - // vertex 2 - btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v, - &btMprSimplexPoint(portal, 1)->v); - if (btMprIsZero(btMprVec3Len2(&dir))){ - if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org)){ - // origin lies on v1 - return 1; - }else{ - // origin lies on v0-v1 segment - return 2; - } - } - - btMprVec3Normalize(&dir); - btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 2)); - - - - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir); - if (btMprIsZero(dot) || dot < 0.f) - return -1; - - btMprSimplexSetSize(portal, 3); - - // vertex 3 direction - btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Cross(&dir, &va, &vb); - btMprVec3Normalize(&dir); - - // it is better to form portal faces to be oriented "outside" origin - dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v); - if (dot > 0.f){ - btMprSimplexSwap(portal, 1, 2); - btMprVec3Scale(&dir, -1.f); - } - - while (btMprSimplexSize(portal) < 4){ - btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 3)); - - dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir); - if (btMprIsZero(dot) || dot < 0.f) - return -1; - - cont = 0; - - // test if origin is outside (v1, v0, v3) - set v2 as v3 and - // continue - btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 3)->v); - dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); - if (dot < 0.f && !btMprIsZero(dot)){ - btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3)); - cont = 1; - } - - if (!cont){ - // test if origin is outside (v3, v0, v2) - set v1 as v3 and - // continue - btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v, - &btMprSimplexPoint(portal, 2)->v); - dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); - if (dot < 0.f && !btMprIsZero(dot)){ - btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3)); - cont = 1; - } - } - - if (cont){ - btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 0)->v); - btMprVec3Cross(&dir, &va, &vb); - btMprVec3Normalize(&dir); - }else{ - btMprSimplexSetSize(portal, 4); - } - } - - return 0; -} - -template -static int btRefinePortal(const btConvexTemplate& a, const btConvexTemplate& b,const btMprCollisionDescription& colDesc, - btMprSimplex_t *portal) -{ - btVector3 dir; - btMprSupport_t v4; - - for (int i=0;iv, - &btMprSimplexPoint(portal, 2)->v); - b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); - - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, - &btMprSimplexPoint(portal, 2)->v); - b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); - - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v, - &btMprSimplexPoint(portal, 1)->v); - b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); - - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 1)->v); - b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); - - sum = b[0] + b[1] + b[2] + b[3]; - - if (btMprIsZero(sum) || sum < 0.f){ - b[0] = 0.f; - - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, - &btMprSimplexPoint(portal, 3)->v); - b[1] = btMprVec3Dot(&vec, &dir); - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, - &btMprSimplexPoint(portal, 1)->v); - b[2] = btMprVec3Dot(&vec, &dir); - btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v, - &btMprSimplexPoint(portal, 2)->v); - b[3] = btMprVec3Dot(&vec, &dir); - - sum = b[1] + b[2] + b[3]; - } - - inv = 1.f / sum; - - btMprVec3Copy(&p1, origin); - btMprVec3Copy(&p2, origin); - for (i = 0; i < 4; i++){ - btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1); - btMprVec3Scale(&vec, b[i]); - btMprVec3Add(&p1, &vec); - - btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2); - btMprVec3Scale(&vec, b[i]); - btMprVec3Add(&p2, &vec); - } - btMprVec3Scale(&p1, inv); - btMprVec3Scale(&p2, inv); -#ifdef MPR_AVERAGE_CONTACT_POSITIONS - btMprVec3Copy(pos, &p1); - btMprVec3Add(pos, &p2); - btMprVec3Scale(pos, 0.5); -#else - btMprVec3Copy(pos, &p2); -#endif//MPR_AVERAGE_CONTACT_POSITIONS -} - -inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b) -{ - btVector3 ab; - btMprVec3Sub2(&ab, a, b); - return btMprVec3Len2(&ab); -} - -inline float _btMprVec3PointSegmentDist2(const btVector3 *P, - const btVector3 *x0, - const btVector3 *b, - btVector3 *witness) -{ - // The computation comes from solving equation of segment: - // S(t) = x0 + t.d - // where - x0 is initial point of segment - // - d is direction of segment from x0 (|d| > 0) - // - t belongs to <0, 1> interval - // - // Than, distance from a segment to some point P can be expressed: - // D(t) = |x0 + t.d - P|^2 - // which is distance from any point on segment. Minimization - // of this function brings distance from P to segment. - // Minimization of D(t) leads to simple quadratic equation that's - // solving is straightforward. - // - // Bonus of this method is witness point for free. - - float dist, t; - btVector3 d, a; - - // direction of segment - btMprVec3Sub2(&d, b, x0); - - // precompute vector from P to x0 - btMprVec3Sub2(&a, x0, P); - - t = -1.f * btMprVec3Dot(&a, &d); - t /= btMprVec3Len2(&d); - - if (t < 0.f || btMprIsZero(t)){ - dist = btMprVec3Dist2(x0, P); - if (witness) - btMprVec3Copy(witness, x0); - }else if (t > 1.f || btMprEq(t, 1.f)){ - dist = btMprVec3Dist2(b, P); - if (witness) - btMprVec3Copy(witness, b); - }else{ - if (witness){ - btMprVec3Copy(witness, &d); - btMprVec3Scale(witness, t); - btMprVec3Add(witness, x0); - dist = btMprVec3Dist2(witness, P); - }else{ - // recycling variables - btMprVec3Scale(&d, t); - btMprVec3Add(&d, &a); - dist = btMprVec3Len2(&d); - } - } - - return dist; -} - - - -inline float btMprVec3PointTriDist2(const btVector3 *P, - const btVector3 *x0, const btVector3 *B, - const btVector3 *C, - btVector3 *witness) -{ - // Computation comes from analytic expression for triangle (x0, B, C) - // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and - // Then equation for distance is: - // D(s, t) = | T(s, t) - P |^2 - // This leads to minimization of quadratic function of two variables. - // The solution from is taken only if s is between 0 and 1, t is - // between 0 and 1 and t + s < 1, otherwise distance from segment is - // computed. - - btVector3 d1, d2, a; - float u, v, w, p, q, r; - float s, t, dist, dist2; - btVector3 witness2; - - btMprVec3Sub2(&d1, B, x0); - btMprVec3Sub2(&d2, C, x0); - btMprVec3Sub2(&a, x0, P); - - u = btMprVec3Dot(&a, &a); - v = btMprVec3Dot(&d1, &d1); - w = btMprVec3Dot(&d2, &d2); - p = btMprVec3Dot(&a, &d1); - q = btMprVec3Dot(&a, &d2); - r = btMprVec3Dot(&d1, &d2); - - btScalar div = (w * v - r * r); - if (btMprIsZero(div)) - { - s=-1; - } else - { - s = (q * r - w * p) / div; - t = (-s * r - q) / w; - } - - if ((btMprIsZero(s) || s > 0.f) - && (btMprEq(s, 1.f) || s < 1.f) - && (btMprIsZero(t) || t > 0.f) - && (btMprEq(t, 1.f) || t < 1.f) - && (btMprEq(t + s, 1.f) || t + s < 1.f)){ - - if (witness){ - btMprVec3Scale(&d1, s); - btMprVec3Scale(&d2, t); - btMprVec3Copy(witness, x0); - btMprVec3Add(witness, &d1); - btMprVec3Add(witness, &d2); - - dist = btMprVec3Dist2(witness, P); - }else{ - dist = s * s * v; - dist += t * t * w; - dist += 2.f * s * t * r; - dist += 2.f * s * p; - dist += 2.f * t * q; - dist += u; - } - }else{ - dist = _btMprVec3PointSegmentDist2(P, x0, B, witness); - - dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2); - if (dist2 < dist){ - dist = dist2; - if (witness) - btMprVec3Copy(witness, &witness2); - } - - dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2); - if (dist2 < dist){ - dist = dist2; - if (witness) - btMprVec3Copy(witness, &witness2); - } - } - - return dist; -} - -template -static void btFindPenetr(const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - btMprSimplex_t *portal, - float *depth, btVector3 *pdir, btVector3 *pos) -{ - btVector3 dir; - btMprSupport_t v4; - unsigned long iterations; - - btVector3 zero = btVector3(0,0,0); - btVector3* origin = &zero; - - - iterations = 1UL; - for (int i=0;i find penetration info - if (portalReachTolerance(portal, &v4, &dir) - || iterations ==BT_MPR_MAX_ITERATIONS) - { - *depth = btMprVec3PointTriDist2(origin,&btMprSimplexPoint(portal, 1)->v,&btMprSimplexPoint(portal, 2)->v,&btMprSimplexPoint(portal, 3)->v,pdir); - *depth = BT_MPR_SQRT(*depth); - - if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z())) - { - - *pdir = dir; - } - btMprVec3Normalize(pdir); - - // barycentric coordinates: - btFindPos(portal, pos); - - - return; - } - - btExpandPortal(portal, &v4); - - iterations++; - } -} - -static void btFindPenetrTouch(btMprSimplex_t *portal,float *depth, btVector3 *dir, btVector3 *pos) -{ - // Touching contact on portal's v1 - so depth is zero and direction - // is unimportant and pos can be guessed - *depth = 0.f; - btVector3 zero = btVector3(0,0,0); - btVector3* origin = &zero; - - - btMprVec3Copy(dir, origin); -#ifdef MPR_AVERAGE_CONTACT_POSITIONS - btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); - btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); - btMprVec3Scale(pos, 0.5); -#else - btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); -#endif -} - -static void btFindPenetrSegment(btMprSimplex_t *portal, - float *depth, btVector3 *dir, btVector3 *pos) -{ - - // Origin lies on v0-v1 segment. - // Depth is distance to v1, direction also and position must be - // computed -#ifdef MPR_AVERAGE_CONTACT_POSITIONS - btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); - btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); - btMprVec3Scale(pos, 0.5f); -#else - btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); -#endif//MPR_AVERAGE_CONTACT_POSITIONS - - btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v); - *depth = BT_MPR_SQRT(btMprVec3Len2(dir)); - btMprVec3Normalize(dir); - - -} - - -template -inline int btMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, - const btMprCollisionDescription& colDesc, - float *depthOut, btVector3* dirOut, btVector3* posOut) -{ - - btMprSimplex_t portal; - - - // Phase 1: Portal discovery - int result = btDiscoverPortal(a,b,colDesc, &portal); - - - //sepAxis[pairIndex] = *pdir;//or -dir? - - switch (result) - { - case 0: - { - // Phase 2: Portal refinement - - result = btRefinePortal(a,b,colDesc, &portal); - if (result < 0) - return -1; - - // Phase 3. Penetration info - btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut); - - - break; - } - case 1: - { - // Touching contact on portal's v1. - btFindPenetrTouch(&portal, depthOut, dirOut, posOut); - result=0; - break; - } - case 2: - { - - btFindPenetrSegment( &portal, depthOut, dirOut, posOut); - result=0; - break; - } - default: - { - //if (res < 0) - //{ - // Origin isn't inside portal - no collision. - result = -1; - //} - } - }; - - return result; -}; - - -template -inline int btComputeMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, const - btMprCollisionDescription& colDesc, btMprDistanceTemplate* distInfo) -{ - btVector3 dir,pos; - float depth; - - int res = btMprPenetration(a,b,colDesc,&depth, &dir, &pos); - if (res==0) - { - distInfo->m_distance = -depth; - distInfo->m_pointOnB = pos; - distInfo->m_normalBtoA = -dir; - distInfo->m_pointOnA = pos-distInfo->m_distance*dir; - return 0; - } - - return -1; -} - - - -#endif //BT_MPR_PENETRATION_H + +/*** + * --------------------------------- + * Copyright (c)2012 Daniel Fiser + * + * This file was ported from mpr.c file, part of libccd. + * The Minkoski Portal Refinement implementation was ported + * to OpenCL by Erwin Coumans for the Bullet 3 Physics library. + * The original MPR idea and implementation is by Gary Snethen + * in XenoCollide, see http://github.com/erwincoumans/xenocollide + * + * Distributed under the OSI-approved BSD License (the "License"); + * see . + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + +///2014 Oct, Erwin Coumans, Use templates to avoid void* casts + +#ifndef BT_MPR_PENETRATION_H +#define BT_MPR_PENETRATION_H + +#define BT_DEBUG_MPR1 + +#include "LinearMath/btTransform.h" +#include "LinearMath/btAlignedObjectArray.h" + +//#define MPR_AVERAGE_CONTACT_POSITIONS + + +struct btMprCollisionDescription +{ + btVector3 m_firstDir; + int m_maxGjkIterations; + btScalar m_maximumDistanceSquared; + btScalar m_gjkRelError2; + + btMprCollisionDescription() + : m_firstDir(0,1,0), + m_maxGjkIterations(1000), + m_maximumDistanceSquared(1e30f), + m_gjkRelError2(1.0e-6) + { + } + virtual ~btMprCollisionDescription() + { + } +}; + +struct btMprDistanceInfo +{ + btVector3 m_pointOnA; + btVector3 m_pointOnB; + btVector3 m_normalBtoA; + btScalar m_distance; +}; + +#ifdef __cplusplus +#define BT_MPR_SQRT sqrtf +#else +#define BT_MPR_SQRT sqrt +#endif +#define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y)) +#define BT_MPR_FABS fabs + +#define BT_MPR_TOLERANCE 1E-6f +#define BT_MPR_MAX_ITERATIONS 1000 + +struct _btMprSupport_t +{ + btVector3 v; //!< Support point in minkowski sum + btVector3 v1; //!< Support point in obj1 + btVector3 v2; //!< Support point in obj2 +}; +typedef struct _btMprSupport_t btMprSupport_t; + +struct _btMprSimplex_t +{ + btMprSupport_t ps[4]; + int last; //!< index of last added point +}; +typedef struct _btMprSimplex_t btMprSimplex_t; + +inline btMprSupport_t* btMprSimplexPointW(btMprSimplex_t *s, int idx) +{ + return &s->ps[idx]; +} + +inline void btMprSimplexSetSize(btMprSimplex_t *s, int size) +{ + s->last = size - 1; +} + +#ifdef DEBUG_MPR +inline void btPrintPortalVertex(_btMprSimplex_t* portal, int index) +{ + printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(),portal->ps[index].v.y(),portal->ps[index].v.z(), + portal->ps[index].v1.x(),portal->ps[index].v1.y(),portal->ps[index].v1.z(), + portal->ps[index].v2.x(),portal->ps[index].v2.y(),portal->ps[index].v2.z()); +} +#endif //DEBUG_MPR + + + + +inline int btMprSimplexSize(const btMprSimplex_t *s) +{ + return s->last + 1; +} + + +inline const btMprSupport_t* btMprSimplexPoint(const btMprSimplex_t* s, int idx) +{ + // here is no check on boundaries + return &s->ps[idx]; +} + +inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s) +{ + *d = *s; +} + +inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a) +{ + btMprSupportCopy(s->ps + pos, a); +} + + +inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2) +{ + btMprSupport_t supp; + + btMprSupportCopy(&supp, &s->ps[pos1]); + btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]); + btMprSupportCopy(&s->ps[pos2], &supp); +} + + +inline int btMprIsZero(float val) +{ + return BT_MPR_FABS(val) < FLT_EPSILON; +} + + + +inline int btMprEq(float _a, float _b) +{ + float ab; + float a, b; + + ab = BT_MPR_FABS(_a - _b); + if (BT_MPR_FABS(ab) < FLT_EPSILON) + return 1; + + a = BT_MPR_FABS(_a); + b = BT_MPR_FABS(_b); + if (b > a){ + return ab < FLT_EPSILON * b; + }else{ + return ab < FLT_EPSILON * a; + } +} + + +inline int btMprVec3Eq(const btVector3* a, const btVector3 *b) +{ + return btMprEq((*a).x(), (*b).x()) + && btMprEq((*a).y(), (*b).y()) + && btMprEq((*a).z(), (*b).z()); +} + + + + + + + + + + + +template +inline void btFindOrigin(const btConvexTemplate& a, const btConvexTemplate& b, const btMprCollisionDescription& colDesc,btMprSupport_t *center) +{ + + center->v1 = a.getObjectCenterInWorld(); + center->v2 = b.getObjectCenterInWorld(); + center->v = center->v1 - center->v2; +} + +inline void btMprVec3Set(btVector3 *v, float x, float y, float z) +{ + v->setValue(x,y,z); +} + +inline void btMprVec3Add(btVector3 *v, const btVector3 *w) +{ + *v += *w; +} + +inline void btMprVec3Copy(btVector3 *v, const btVector3 *w) +{ + *v = *w; +} + +inline void btMprVec3Scale(btVector3 *d, float k) +{ + *d *= k; +} + +inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b) +{ + float dot; + + dot = btDot(*a,*b); + return dot; +} + + +inline float btMprVec3Len2(const btVector3 *v) +{ + return btMprVec3Dot(v, v); +} + +inline void btMprVec3Normalize(btVector3 *d) +{ + float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d)); + btMprVec3Scale(d, k); +} + +inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b) +{ + *d = btCross(*a,*b); + +} + + +inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) +{ + *d = *v - *w; +} + +inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir) +{ + btVector3 v2v1, v3v1; + + btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 1)->v); + btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v, + &btMprSimplexPoint(portal, 1)->v); + btMprVec3Cross(dir, &v2v1, &v3v1); + btMprVec3Normalize(dir); +} + + +inline int portalEncapsulesOrigin(const btMprSimplex_t *portal, + const btVector3 *dir) +{ + float dot; + dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v); + return btMprIsZero(dot) || dot > 0.f; +} + +inline int portalReachTolerance(const btMprSimplex_t *portal, + const btMprSupport_t *v4, + const btVector3 *dir) +{ + float dv1, dv2, dv3, dv4; + float dot1, dot2, dot3; + + // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} + + dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir); + dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir); + dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir); + dv4 = btMprVec3Dot(&v4->v, dir); + + dot1 = dv4 - dv1; + dot2 = dv4 - dv2; + dot3 = dv4 - dv3; + + dot1 = BT_MPR_FMIN(dot1, dot2); + dot1 = BT_MPR_FMIN(dot1, dot3); + + return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE; +} + +inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal, + const btMprSupport_t *v4, + const btVector3 *dir) +{ + float dot; + dot = btMprVec3Dot(&v4->v, dir); + return btMprIsZero(dot) || dot > 0.f; +} + +inline void btExpandPortal(btMprSimplex_t *portal, + const btMprSupport_t *v4) +{ + float dot; + btVector3 v4v0; + + btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v); + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0); + if (dot > 0.f){ + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0); + if (dot > 0.f){ + btMprSimplexSet(portal, 1, v4); + }else{ + btMprSimplexSet(portal, 3, v4); + } + }else{ + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0); + if (dot > 0.f){ + btMprSimplexSet(portal, 2, v4); + }else{ + btMprSimplexSet(portal, 1, v4); + } + } +} +template +inline void btMprSupport(const btConvexTemplate& a, const btConvexTemplate& b, + const btMprCollisionDescription& colDesc, + const btVector3& dir, btMprSupport_t *supp) +{ + btVector3 seperatingAxisInA = dir* a.getWorldTransform().getBasis(); + btVector3 seperatingAxisInB = -dir* b.getWorldTransform().getBasis(); + + btVector3 pInA = a.getLocalSupportWithMargin(seperatingAxisInA); + btVector3 qInB = b.getLocalSupportWithMargin(seperatingAxisInB); + + supp->v1 = a.getWorldTransform()(pInA); + supp->v2 = b.getWorldTransform()(qInB); + supp->v = supp->v1 - supp->v2; +} + + +template +static int btDiscoverPortal(const btConvexTemplate& a, const btConvexTemplate& b, + const btMprCollisionDescription& colDesc, + btMprSimplex_t *portal) +{ + btVector3 dir, va, vb; + float dot; + int cont; + + + + // vertex 0 is center of portal + btFindOrigin(a,b,colDesc, btMprSimplexPointW(portal, 0)); + + + // vertex 0 is center of portal + btMprSimplexSetSize(portal, 1); + + + + btVector3 zero = btVector3(0,0,0); + btVector3* org = &zero; + + if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org)){ + // Portal's center lies on origin (0,0,0) => we know that objects + // intersect but we would need to know penetration info. + // So move center little bit... + btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f); + btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va); + } + + + // vertex 1 = support in direction of origin + btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v); + btMprVec3Scale(&dir, -1.f); + btMprVec3Normalize(&dir); + + + btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 1)); + + btMprSimplexSetSize(portal, 2); + + // test if origin isn't outside of v1 + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir); + + + if (btMprIsZero(dot) || dot < 0.f) + return -1; + + + // vertex 2 + btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v, + &btMprSimplexPoint(portal, 1)->v); + if (btMprIsZero(btMprVec3Len2(&dir))){ + if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org)){ + // origin lies on v1 + return 1; + }else{ + // origin lies on v0-v1 segment + return 2; + } + } + + btMprVec3Normalize(&dir); + btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 2)); + + + + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir); + if (btMprIsZero(dot) || dot < 0.f) + return -1; + + btMprSimplexSetSize(portal, 3); + + // vertex 3 direction + btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 0)->v); + btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 0)->v); + btMprVec3Cross(&dir, &va, &vb); + btMprVec3Normalize(&dir); + + // it is better to form portal faces to be oriented "outside" origin + dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v); + if (dot > 0.f){ + btMprSimplexSwap(portal, 1, 2); + btMprVec3Scale(&dir, -1.f); + } + + while (btMprSimplexSize(portal) < 4){ + btMprSupport(a,b,colDesc, dir, btMprSimplexPointW(portal, 3)); + + dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir); + if (btMprIsZero(dot) || dot < 0.f) + return -1; + + cont = 0; + + // test if origin is outside (v1, v0, v3) - set v2 as v3 and + // continue + btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 3)->v); + dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !btMprIsZero(dot)){ + btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3)); + cont = 1; + } + + if (!cont){ + // test if origin is outside (v3, v0, v2) - set v1 as v3 and + // continue + btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v, + &btMprSimplexPoint(portal, 2)->v); + dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !btMprIsZero(dot)){ + btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3)); + cont = 1; + } + } + + if (cont){ + btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 0)->v); + btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 0)->v); + btMprVec3Cross(&dir, &va, &vb); + btMprVec3Normalize(&dir); + }else{ + btMprSimplexSetSize(portal, 4); + } + } + + return 0; +} + +template +static int btRefinePortal(const btConvexTemplate& a, const btConvexTemplate& b,const btMprCollisionDescription& colDesc, + btMprSimplex_t *portal) +{ + btVector3 dir; + btMprSupport_t v4; + + for (int i=0;iv, + &btMprSimplexPoint(portal, 2)->v); + b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); + + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, + &btMprSimplexPoint(portal, 2)->v); + b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); + + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v, + &btMprSimplexPoint(portal, 1)->v); + b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v); + + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 1)->v); + b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v); + + sum = b[0] + b[1] + b[2] + b[3]; + + if (btMprIsZero(sum) || sum < 0.f){ + b[0] = 0.f; + + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v, + &btMprSimplexPoint(portal, 3)->v); + b[1] = btMprVec3Dot(&vec, &dir); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v, + &btMprSimplexPoint(portal, 1)->v); + b[2] = btMprVec3Dot(&vec, &dir); + btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v, + &btMprSimplexPoint(portal, 2)->v); + b[3] = btMprVec3Dot(&vec, &dir); + + sum = b[1] + b[2] + b[3]; + } + + inv = 1.f / sum; + + btMprVec3Copy(&p1, origin); + btMprVec3Copy(&p2, origin); + for (i = 0; i < 4; i++){ + btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1); + btMprVec3Scale(&vec, b[i]); + btMprVec3Add(&p1, &vec); + + btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2); + btMprVec3Scale(&vec, b[i]); + btMprVec3Add(&p2, &vec); + } + btMprVec3Scale(&p1, inv); + btMprVec3Scale(&p2, inv); +#ifdef MPR_AVERAGE_CONTACT_POSITIONS + btMprVec3Copy(pos, &p1); + btMprVec3Add(pos, &p2); + btMprVec3Scale(pos, 0.5); +#else + btMprVec3Copy(pos, &p2); +#endif//MPR_AVERAGE_CONTACT_POSITIONS +} + +inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b) +{ + btVector3 ab; + btMprVec3Sub2(&ab, a, b); + return btMprVec3Len2(&ab); +} + +inline float _btMprVec3PointSegmentDist2(const btVector3 *P, + const btVector3 *x0, + const btVector3 *b, + btVector3 *witness) +{ + // The computation comes from solving equation of segment: + // S(t) = x0 + t.d + // where - x0 is initial point of segment + // - d is direction of segment from x0 (|d| > 0) + // - t belongs to <0, 1> interval + // + // Than, distance from a segment to some point P can be expressed: + // D(t) = |x0 + t.d - P|^2 + // which is distance from any point on segment. Minimization + // of this function brings distance from P to segment. + // Minimization of D(t) leads to simple quadratic equation that's + // solving is straightforward. + // + // Bonus of this method is witness point for free. + + float dist, t; + btVector3 d, a; + + // direction of segment + btMprVec3Sub2(&d, b, x0); + + // precompute vector from P to x0 + btMprVec3Sub2(&a, x0, P); + + t = -1.f * btMprVec3Dot(&a, &d); + t /= btMprVec3Len2(&d); + + if (t < 0.f || btMprIsZero(t)){ + dist = btMprVec3Dist2(x0, P); + if (witness) + btMprVec3Copy(witness, x0); + }else if (t > 1.f || btMprEq(t, 1.f)){ + dist = btMprVec3Dist2(b, P); + if (witness) + btMprVec3Copy(witness, b); + }else{ + if (witness){ + btMprVec3Copy(witness, &d); + btMprVec3Scale(witness, t); + btMprVec3Add(witness, x0); + dist = btMprVec3Dist2(witness, P); + }else{ + // recycling variables + btMprVec3Scale(&d, t); + btMprVec3Add(&d, &a); + dist = btMprVec3Len2(&d); + } + } + + return dist; +} + + + +inline float btMprVec3PointTriDist2(const btVector3 *P, + const btVector3 *x0, const btVector3 *B, + const btVector3 *C, + btVector3 *witness) +{ + // Computation comes from analytic expression for triangle (x0, B, C) + // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and + // Then equation for distance is: + // D(s, t) = | T(s, t) - P |^2 + // This leads to minimization of quadratic function of two variables. + // The solution from is taken only if s is between 0 and 1, t is + // between 0 and 1 and t + s < 1, otherwise distance from segment is + // computed. + + btVector3 d1, d2, a; + float u, v, w, p, q, r; + float s, t, dist, dist2; + btVector3 witness2; + + btMprVec3Sub2(&d1, B, x0); + btMprVec3Sub2(&d2, C, x0); + btMprVec3Sub2(&a, x0, P); + + u = btMprVec3Dot(&a, &a); + v = btMprVec3Dot(&d1, &d1); + w = btMprVec3Dot(&d2, &d2); + p = btMprVec3Dot(&a, &d1); + q = btMprVec3Dot(&a, &d2); + r = btMprVec3Dot(&d1, &d2); + + btScalar div = (w * v - r * r); + if (btMprIsZero(div)) + { + s=-1; + } else + { + s = (q * r - w * p) / div; + t = (-s * r - q) / w; + } + + if ((btMprIsZero(s) || s > 0.f) + && (btMprEq(s, 1.f) || s < 1.f) + && (btMprIsZero(t) || t > 0.f) + && (btMprEq(t, 1.f) || t < 1.f) + && (btMprEq(t + s, 1.f) || t + s < 1.f)){ + + if (witness){ + btMprVec3Scale(&d1, s); + btMprVec3Scale(&d2, t); + btMprVec3Copy(witness, x0); + btMprVec3Add(witness, &d1); + btMprVec3Add(witness, &d2); + + dist = btMprVec3Dist2(witness, P); + }else{ + dist = s * s * v; + dist += t * t * w; + dist += 2.f * s * t * r; + dist += 2.f * s * p; + dist += 2.f * t * q; + dist += u; + } + }else{ + dist = _btMprVec3PointSegmentDist2(P, x0, B, witness); + + dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + btMprVec3Copy(witness, &witness2); + } + + dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + btMprVec3Copy(witness, &witness2); + } + } + + return dist; +} + +template +static void btFindPenetr(const btConvexTemplate& a, const btConvexTemplate& b, + const btMprCollisionDescription& colDesc, + btMprSimplex_t *portal, + float *depth, btVector3 *pdir, btVector3 *pos) +{ + btVector3 dir; + btMprSupport_t v4; + unsigned long iterations; + + btVector3 zero = btVector3(0,0,0); + btVector3* origin = &zero; + + + iterations = 1UL; + for (int i=0;i find penetration info + if (portalReachTolerance(portal, &v4, &dir) + || iterations ==BT_MPR_MAX_ITERATIONS) + { + *depth = btMprVec3PointTriDist2(origin,&btMprSimplexPoint(portal, 1)->v,&btMprSimplexPoint(portal, 2)->v,&btMprSimplexPoint(portal, 3)->v,pdir); + *depth = BT_MPR_SQRT(*depth); + + if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z())) + { + + *pdir = dir; + } + btMprVec3Normalize(pdir); + + // barycentric coordinates: + btFindPos(portal, pos); + + + return; + } + + btExpandPortal(portal, &v4); + + iterations++; + } +} + +static void btFindPenetrTouch(btMprSimplex_t *portal,float *depth, btVector3 *dir, btVector3 *pos) +{ + // Touching contact on portal's v1 - so depth is zero and direction + // is unimportant and pos can be guessed + *depth = 0.f; + btVector3 zero = btVector3(0,0,0); + btVector3* origin = &zero; + + + btMprVec3Copy(dir, origin); +#ifdef MPR_AVERAGE_CONTACT_POSITIONS + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); + btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); + btMprVec3Scale(pos, 0.5); +#else + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); +#endif +} + +static void btFindPenetrSegment(btMprSimplex_t *portal, + float *depth, btVector3 *dir, btVector3 *pos) +{ + + // Origin lies on v0-v1 segment. + // Depth is distance to v1, direction also and position must be + // computed +#ifdef MPR_AVERAGE_CONTACT_POSITIONS + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1); + btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2); + btMprVec3Scale(pos, 0.5f); +#else + btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2); +#endif//MPR_AVERAGE_CONTACT_POSITIONS + + btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v); + *depth = BT_MPR_SQRT(btMprVec3Len2(dir)); + btMprVec3Normalize(dir); + + +} + + +template +inline int btMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, + const btMprCollisionDescription& colDesc, + float *depthOut, btVector3* dirOut, btVector3* posOut) +{ + + btMprSimplex_t portal; + + + // Phase 1: Portal discovery + int result = btDiscoverPortal(a,b,colDesc, &portal); + + + //sepAxis[pairIndex] = *pdir;//or -dir? + + switch (result) + { + case 0: + { + // Phase 2: Portal refinement + + result = btRefinePortal(a,b,colDesc, &portal); + if (result < 0) + return -1; + + // Phase 3. Penetration info + btFindPenetr(a,b,colDesc, &portal, depthOut, dirOut, posOut); + + + break; + } + case 1: + { + // Touching contact on portal's v1. + btFindPenetrTouch(&portal, depthOut, dirOut, posOut); + result=0; + break; + } + case 2: + { + + btFindPenetrSegment( &portal, depthOut, dirOut, posOut); + result=0; + break; + } + default: + { + //if (res < 0) + //{ + // Origin isn't inside portal - no collision. + result = -1; + //} + } + }; + + return result; +}; + + +template +inline int btComputeMprPenetration( const btConvexTemplate& a, const btConvexTemplate& b, const + btMprCollisionDescription& colDesc, btMprDistanceTemplate* distInfo) +{ + btVector3 dir,pos; + float depth; + + int res = btMprPenetration(a,b,colDesc,&depth, &dir, &pos); + if (res==0) + { + distInfo->m_distance = -depth; + distInfo->m_pointOnB = pos; + distInfo->m_normalBtoA = -dir; + distInfo->m_pointOnA = pos-distInfo->m_distance*dir; + return 0; + } + + return -1; +} + + + +#endif //BT_MPR_PENETRATION_H diff --git a/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp new file mode 100644 index 000000000000..9603a8bbdcae --- /dev/null +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -0,0 +1,459 @@ +/* +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 "btPersistentManifold.h" +#include "LinearMath/btTransform.h" +#include "LinearMath/btSerializer.h" + +#ifdef BT_USE_DOUBLE_PRECISION +#define btCollisionObjectData btCollisionObjectDoubleData +#else +#define btCollisionObjectData btCollisionObjectFloatData +#endif + +btScalar gContactBreakingThreshold = btScalar(0.02); +ContactDestroyedCallback gContactDestroyedCallback = 0; +ContactProcessedCallback gContactProcessedCallback = 0; +ContactStartedCallback gContactStartedCallback = 0; +ContactEndedCallback gContactEndedCallback = 0; +///gContactCalcArea3Points will approximate the convex hull area using 3 points +///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower +bool gContactCalcArea3Points = true; + + +btPersistentManifold::btPersistentManifold() +:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), +m_body0(0), +m_body1(0), +m_cachedPoints (0), +m_companionIdA(0), +m_companionIdB(0), +m_index1a(0) +{ +} + + + + +#ifdef DEBUG_PERSISTENCY +#include +void btPersistentManifold::DebugPersistency() +{ + int i; + printf("DebugPersistency : numPoints %d\n",m_cachedPoints); + for (i=0;i1) + 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 + } + + +} + +static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3) +{ + // It calculates possible 3 area constructed from random 4 points and returns the biggest one. + + btVector3 a[3],b[3]; + a[0] = p0 - p1; + a[1] = p0 - p2; + a[2] = p0 - p3; + b[0] = p2 - p3; + b[1] = p1 - p3; + b[2] = p1 - p2; + + //todo: Following 3 cross production can be easily optimized by SIMD. + btVector3 tmp0 = a[0].cross(b[0]); + btVector3 tmp1 = a[1].cross(b[1]); + btVector3 tmp2 = a[2].cross(b[2]); + + return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2()); +} + +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 (gContactCalcArea3Points) + { + 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(); + } + } + else + { + if(maxPenetrationIndex != 0) { + res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); + } + + if(maxPenetrationIndex != 1) { + res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); + } + + if(maxPenetrationIndex != 2) { + res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA); + } + + if(maxPenetrationIndex != 3) { + res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA); + } + } + 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, bool isPredictive) +{ + if (!isPredictive) + { + 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 + { + //todo: friction anchor may require the contact to be around a bit longer + //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,(void*)m_body0,(void*)m_body1); + } + } + } +#ifdef DEBUG_PERSISTENCY + DebugPersistency(); +#endif // +} + + +int btPersistentManifold::calculateSerializeBufferSize() const +{ + return sizeof(btPersistentManifoldData); +} + +const char* btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const +{ + btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer; + memset(dataOut, 0, sizeof(btPersistentManifoldData)); + + dataOut->m_body0 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody0()); + dataOut->m_body1 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody1()); + dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold(); + dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold(); + dataOut->m_numCachedPoints = manifold->getNumContacts(); + dataOut->m_companionIdA = manifold->m_companionIdA; + dataOut->m_companionIdB = manifold->m_companionIdB; + dataOut->m_index1a = manifold->m_index1a; + dataOut->m_objectType = manifold->m_objectType; + + for (int i = 0; i < this->getNumContacts(); i++) + { + const btManifoldPoint& pt = manifold->getContactPoint(i); + dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse; + dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1; + dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2; + pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]); + pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]); + pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]); + dataOut->m_pointCacheDistance[i] = pt.m_distance1; + dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1; + dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1; + dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime; + dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM; + dataOut->m_pointCacheContactERP[i] = pt.m_contactERP; + dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM; + dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags; + dataOut->m_pointCacheIndex0[i] = pt.m_index0; + dataOut->m_pointCacheIndex1[i] = pt.m_index1; + dataOut->m_pointCachePartId0[i] = pt.m_partId0; + dataOut->m_pointCachePartId1[i] = pt.m_partId1; + pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]); + pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]); + dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction; + pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]); + pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]); + dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction; + dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction; + dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution; + dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1; + dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2; + } + return btPersistentManifoldDataName; +} + +void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr) +{ + m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold; + m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold; + m_cachedPoints = manifoldDataPtr->m_numCachedPoints; + m_companionIdA = manifoldDataPtr->m_companionIdA; + m_companionIdB = manifoldDataPtr->m_companionIdB; + //m_index1a = manifoldDataPtr->m_index1a; + m_objectType = manifoldDataPtr->m_objectType; + + for (int i = 0; i < this->getNumContacts(); i++) + { + btManifoldPoint& pt = m_pointCache[i]; + + pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i]; + pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i]; + pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i]; + pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]); + pt.m_localPointB.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointB[i]); + pt.m_normalWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]); + pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i]; + pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i]; + pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i]; + pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i]; + pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i]; + pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i]; + pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i]; + pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i]; + pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i]; + pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i]; + pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i]; + pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i]; + pt.m_positionWorldOnA.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnA[i]); + pt.m_positionWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnB[i]); + pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i]; + pt.m_lateralFrictionDir1.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]); + pt.m_lateralFrictionDir2.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]); + pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i]; + pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i]; + pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i]; + pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i]; + pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i]; + } + +} + +void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr) +{ + m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold; + m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold; + m_cachedPoints = manifoldDataPtr->m_numCachedPoints; + m_companionIdA = manifoldDataPtr->m_companionIdA; + m_companionIdB = manifoldDataPtr->m_companionIdB; + //m_index1a = manifoldDataPtr->m_index1a; + m_objectType = manifoldDataPtr->m_objectType; + + for (int i = 0; i < this->getNumContacts(); i++) + { + btManifoldPoint& pt = m_pointCache[i]; + + pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i]; + pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i]; + pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i]; + pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]); + pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]); + pt.m_normalWorldOnB.deSerialize(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]); + pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i]; + pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i]; + pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i]; + pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i]; + pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i]; + pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i]; + pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i]; + pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i]; + pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i]; + pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i]; + pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i]; + pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i]; + pt.m_positionWorldOnA.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnA[i]); + pt.m_positionWorldOnB.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnB[i]); + pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i]; + pt.m_lateralFrictionDir1.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]); + pt.m_lateralFrictionDir2.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]); + pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i]; + pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i]; + pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i]; + pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i]; + pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i]; + } + +} \ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h similarity index 51% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 5026397f67fd..67be0c48eba9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -24,14 +24,24 @@ class btCollisionObject; #include "LinearMath/btAlignedAllocator.h" struct btCollisionResult; +struct btCollisionObjectDoubleData; +struct btCollisionObjectFloatData; ///maximum contact breaking and merging threshold extern btScalar gContactBreakingThreshold; +#ifndef SWIG +class btPersistentManifold; + typedef bool (*ContactDestroyedCallback)(void* userPersistentData); typedef bool (*ContactProcessedCallback)(btManifoldPoint& cp,void* body0,void* body1); +typedef void (*ContactStartedCallback)(btPersistentManifold* const &manifold); +typedef void (*ContactEndedCallback)(btPersistentManifold* const &manifold); extern ContactDestroyedCallback gContactDestroyedCallback; extern ContactProcessedCallback gContactProcessedCallback; +extern ContactStartedCallback gContactStartedCallback; +extern ContactEndedCallback gContactEndedCallback; +#endif //SWIG //the enum starts at 1024 to avoid type conflicts with btTypedConstraint enum btContactManifoldTypes @@ -51,8 +61,8 @@ enum btContactManifoldTypes ///note that some pairs of objects might have more then one contact manifold. -ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject -//ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject +//ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject +ATTRIBUTE_ALIGNED16( class) btPersistentManifold : public btTypedObject { btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]; @@ -87,7 +97,10 @@ ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), m_body0(body0),m_body1(body1),m_cachedPoints(0), m_contactBreakingThreshold(contactBreakingThreshold), - m_contactProcessingThreshold(contactProcessingThreshold) + m_contactProcessingThreshold(contactProcessingThreshold), + m_companionIdA(0), + m_companionIdB(0), + m_index1a(0) { } @@ -163,7 +176,7 @@ ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject //get rid of duplicated userPersistentData pointer m_pointCache[lastUsedIndex].m_userPersistentData = 0; m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f; - m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false; + m_pointCache[lastUsedIndex].m_contactPointFlags = 0; m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f; m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f; m_pointCache[lastUsedIndex].m_lifeTime = 0; @@ -171,41 +184,60 @@ ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0); m_cachedPoints--; + + if (gContactEndedCallback && m_cachedPoints == 0) + { + gContactEndedCallback(this); + } } - void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex) + void replaceContactPoint(const btManifoldPoint& newPoint, int insertIndex) { btAssert(validContactDistance(newPoint)); #define MAINTAIN_PERSISTENCY 1 #ifdef MAINTAIN_PERSISTENCY - int lifeTime = m_pointCache[insertIndex].getLifeTime(); - btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse; - btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; - btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; -// bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized; - - - - btAssert(lifeTime>=0); - void* cache = m_pointCache[insertIndex].m_userPersistentData; - - m_pointCache[insertIndex] = newPoint; + int lifeTime = m_pointCache[insertIndex].getLifeTime(); + btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse; + btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1; + btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2; + + bool replacePoint = true; + ///we keep existing contact points for friction anchors + ///if the friction force is within the Coulomb friction cone + if (newPoint.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) + { + // printf("appliedImpulse=%f\n", appliedImpulse); + // printf("appliedLateralImpulse1=%f\n", appliedLateralImpulse1); + // printf("appliedLateralImpulse2=%f\n", appliedLateralImpulse2); + // printf("mu = %f\n", m_pointCache[insertIndex].m_combinedFriction); + btScalar mu = m_pointCache[insertIndex].m_combinedFriction; + btScalar eps = 0; //we could allow to enlarge or shrink the tolerance to check against the friction cone a bit, say 1e-7 + btScalar a = appliedLateralImpulse1 * appliedLateralImpulse1 + appliedLateralImpulse2 * appliedLateralImpulse2; + btScalar b = eps + mu * appliedImpulse; + b = b * b; + replacePoint = (a) > (b); + } - m_pointCache[insertIndex].m_userPersistentData = cache; - m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; - m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; - m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; - + if (replacePoint) + { + btAssert(lifeTime >= 0); + void* cache = m_pointCache[insertIndex].m_userPersistentData; + + m_pointCache[insertIndex] = newPoint; + m_pointCache[insertIndex].m_userPersistentData = cache; + m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; + m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; + m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2; + } m_pointCache[insertIndex].m_lifeTime = lifeTime; #else clearUserCache(m_pointCache[insertIndex]); m_pointCache[insertIndex] = newPoint; - + #endif } - bool validContactDistance(const btManifoldPoint& pt) const { return pt.m_distance1 <= getContactBreakingThreshold(); @@ -221,13 +253,123 @@ ATTRIBUTE_ALIGNED128( class) btPersistentManifold : public btTypedObject { clearUserCache(m_pointCache[i]); } + + if (gContactEndedCallback && m_cachedPoints) + { + gContactEndedCallback(this); + } m_cachedPoints = 0; } + int calculateSerializeBufferSize() const; + const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const; + void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr); + void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr); + + +}; + + + +struct btPersistentManifoldDoubleData +{ + btVector3DoubleData m_pointCacheLocalPointA[4]; + btVector3DoubleData m_pointCacheLocalPointB[4]; + btVector3DoubleData m_pointCachePositionWorldOnA[4]; + btVector3DoubleData m_pointCachePositionWorldOnB[4]; + btVector3DoubleData m_pointCacheNormalWorldOnB[4]; + btVector3DoubleData m_pointCacheLateralFrictionDir1[4]; + btVector3DoubleData m_pointCacheLateralFrictionDir2[4]; + double m_pointCacheDistance[4]; + double m_pointCacheAppliedImpulse[4]; + double m_pointCacheCombinedFriction[4]; + double m_pointCacheCombinedRollingFriction[4]; + double m_pointCacheCombinedSpinningFriction[4]; + double m_pointCacheCombinedRestitution[4]; + int m_pointCachePartId0[4]; + int m_pointCachePartId1[4]; + int m_pointCacheIndex0[4]; + int m_pointCacheIndex1[4]; + int m_pointCacheContactPointFlags[4]; + double m_pointCacheAppliedImpulseLateral1[4]; + double m_pointCacheAppliedImpulseLateral2[4]; + double m_pointCacheContactMotion1[4]; + double m_pointCacheContactMotion2[4]; + double m_pointCacheContactCFM[4]; + double m_pointCacheCombinedContactStiffness1[4]; + double m_pointCacheContactERP[4]; + double m_pointCacheCombinedContactDamping1[4]; + double m_pointCacheFrictionCFM[4]; + int m_pointCacheLifeTime[4]; + + int m_numCachedPoints; + int m_companionIdA; + int m_companionIdB; + int m_index1a; + + int m_objectType; + double m_contactBreakingThreshold; + double m_contactProcessingThreshold; + int m_padding; + + btCollisionObjectDoubleData *m_body0; + btCollisionObjectDoubleData *m_body1; +}; -} -; +struct btPersistentManifoldFloatData +{ + btVector3FloatData m_pointCacheLocalPointA[4]; + btVector3FloatData m_pointCacheLocalPointB[4]; + btVector3FloatData m_pointCachePositionWorldOnA[4]; + btVector3FloatData m_pointCachePositionWorldOnB[4]; + btVector3FloatData m_pointCacheNormalWorldOnB[4]; + btVector3FloatData m_pointCacheLateralFrictionDir1[4]; + btVector3FloatData m_pointCacheLateralFrictionDir2[4]; + float m_pointCacheDistance[4]; + float m_pointCacheAppliedImpulse[4]; + float m_pointCacheCombinedFriction[4]; + float m_pointCacheCombinedRollingFriction[4]; + float m_pointCacheCombinedSpinningFriction[4]; + float m_pointCacheCombinedRestitution[4]; + int m_pointCachePartId0[4]; + int m_pointCachePartId1[4]; + int m_pointCacheIndex0[4]; + int m_pointCacheIndex1[4]; + int m_pointCacheContactPointFlags[4]; + float m_pointCacheAppliedImpulseLateral1[4]; + float m_pointCacheAppliedImpulseLateral2[4]; + float m_pointCacheContactMotion1[4]; + float m_pointCacheContactMotion2[4]; + float m_pointCacheContactCFM[4]; + float m_pointCacheCombinedContactStiffness1[4]; + float m_pointCacheContactERP[4]; + float m_pointCacheCombinedContactDamping1[4]; + float m_pointCacheFrictionCFM[4]; + int m_pointCacheLifeTime[4]; + + int m_numCachedPoints; + int m_companionIdA; + int m_companionIdB; + int m_index1a; + + int m_objectType; + float m_contactBreakingThreshold; + float m_contactProcessingThreshold; + int m_padding; + + btCollisionObjectFloatData *m_body0; + btCollisionObjectFloatData *m_body1; +}; + +#ifdef BT_USE_DOUBLE_PRECISION +#define btPersistentManifoldData btPersistentManifoldDoubleData +#define btPersistentManifoldDataName "btPersistentManifoldDoubleData" +#else +#define btPersistentManifoldData btPersistentManifoldFloatData +#define btPersistentManifoldDataName "btPersistentManifoldFloatData" +#endif //BT_USE_DOUBLE_PRECISION + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp similarity index 97% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp index d5f4a964bf36..ea380bc5f1b1 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp @@ -411,9 +411,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& return true; } -void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) +void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) { - btVertexArray worldVertsB2; + worldVertsB2.resize(0); btVertexArray* pVtxIn = &worldVertsB1; btVertexArray* pVtxOut = &worldVertsB2; pVtxOut->reserve(pVtxIn->size()); @@ -527,7 +527,7 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin -void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) +void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btVertexArray& worldVertsB1,btVertexArray& worldVertsB2,btDiscreteCollisionDetectorInterface::Result& resultOut) { btVector3 separatingNormal = separatingNormal1.normalized(); @@ -552,7 +552,7 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin } } } - btVertexArray worldVertsB1; + worldVertsB1.resize(0); { const btFace& polyB = hullB.m_faces[closestFaceB]; const int numVertices = polyB.m_indices.size(); @@ -565,6 +565,6 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin if (closestFaceB>=0) - clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut); + clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, worldVertsB2,minDist, maxDist,resultOut); } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h similarity index 79% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h index b87bd4f3245a..30e3db687b48 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h @@ -32,8 +32,11 @@ typedef btAlignedObjectArray btVertexArray; // Clips a face to the back of a plane struct btPolyhedralContactClipping { - static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut); - static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut); + + static void clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btVertexArray& worldVertsB1,btVertexArray& worldVertsB2,btDiscreteCollisionDetectorInterface::Result& resultOut); + + static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1,btVertexArray& worldVertsB2, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut); + static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp similarity index 94% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index ec638f60ba53..08d6e6de8696 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -72,11 +72,18 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( btScalar dist2 = v.length2(); + #ifdef BT_USE_DOUBLE_PRECISION - btScalar epsilon = btScalar(0.0001); + btScalar epsilon = SIMD_EPSILON * 10; #else +//todo: epsilon kept for backward compatibility of unit tests. +//will need to digg deeper to make the algorithm more robust +//since, a large epsilon can cause an early termination with false +//positive results (ray intersections that shouldn't be there) btScalar epsilon = btScalar(0.0001); #endif //BT_USE_DOUBLE_PRECISION + + btVector3 w,p; btScalar VdotR; diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h similarity index 100% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp similarity index 99% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp index 23b4f79cfc20..756373c9b5ee 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -190,9 +190,9 @@ bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() const btVector3& c = m_simplexVectorW[2]; const btVector3& d = m_simplexVectorW[3]; - bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); + bool hasSeparation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); - if (hasSeperation) + if (hasSeparation) { m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h similarity index 97% rename from extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h rename to extern/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h index 2f389e27e3fd..80fd490f4e15 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ b/extern/bullet/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -26,8 +26,12 @@ subject to the following restrictions: ///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure #define BT_USE_EQUAL_VERTEX_THRESHOLD -#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f +#ifdef BT_USE_DOUBLE_PRECISION +#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 1e-12f +#else +#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f +#endif//BT_USE_DOUBLE_PRECISION struct btUsageBitfield{ btUsageBitfield() diff --git a/extern/bullet/src/BulletCollision/premake4.lua b/extern/bullet/src/BulletCollision/premake4.lua new file mode 100644 index 000000000000..6297bf3dfddf --- /dev/null +++ b/extern/bullet/src/BulletCollision/premake4.lua @@ -0,0 +1,23 @@ + project "BulletCollision" + + kind "StaticLib" + if os.is("Linux") then + buildoptions{"-fPIC"} + end + includedirs { + "..", + } + files { + "*.cpp", + "*.h", + "BroadphaseCollision/*.cpp", + "BroadphaseCollision/*.h", + "CollisionDispatch/*.cpp", + "CollisionDispatch/*.h", + "CollisionShapes/*.cpp", + "CollisionShapes/*.h", + "Gimpact/*.cpp", + "Gimpact/*.h", + "NarrowPhaseCollision/*.cpp", + "NarrowPhaseCollision/*.h", + } diff --git a/extern/bullet/src/BulletDynamics/CMakeLists.txt b/extern/bullet/src/BulletDynamics/CMakeLists.txt new file mode 100644 index 000000000000..2eb03c39af63 --- /dev/null +++ b/extern/bullet/src/BulletDynamics/CMakeLists.txt @@ -0,0 +1,173 @@ +INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src ) + + + +SET(BulletDynamics_SRCS + Character/btKinematicCharacterController.cpp + ConstraintSolver/btConeTwistConstraint.cpp + ConstraintSolver/btContactConstraint.cpp + ConstraintSolver/btFixedConstraint.cpp + ConstraintSolver/btGearConstraint.cpp + ConstraintSolver/btGeneric6DofConstraint.cpp + ConstraintSolver/btGeneric6DofSpringConstraint.cpp + ConstraintSolver/btGeneric6DofSpring2Constraint.cpp + ConstraintSolver/btHinge2Constraint.cpp + ConstraintSolver/btHingeConstraint.cpp + ConstraintSolver/btPoint2PointConstraint.cpp + ConstraintSolver/btSequentialImpulseConstraintSolver.cpp + ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp + ConstraintSolver/btBatchedConstraints.cpp + ConstraintSolver/btNNCGConstraintSolver.cpp + ConstraintSolver/btSliderConstraint.cpp + ConstraintSolver/btSolve2LinearConstraint.cpp + ConstraintSolver/btTypedConstraint.cpp + ConstraintSolver/btUniversalConstraint.cpp + Dynamics/btDiscreteDynamicsWorld.cpp + Dynamics/btDiscreteDynamicsWorldMt.cpp + Dynamics/btSimulationIslandManagerMt.cpp + Dynamics/btRigidBody.cpp + Dynamics/btSimpleDynamicsWorld.cpp +# Dynamics/Bullet-C-API.cpp + Vehicle/btRaycastVehicle.cpp + Vehicle/btWheelInfo.cpp + Featherstone/btMultiBody.cpp + Featherstone/btMultiBodyConstraintSolver.cpp + Featherstone/btMultiBodyDynamicsWorld.cpp + Featherstone/btMultiBodyJointLimitConstraint.cpp + Featherstone/btMultiBodyConstraint.cpp + Featherstone/btMultiBodyPoint2Point.cpp + Featherstone/btMultiBodyFixedConstraint.cpp + Featherstone/btMultiBodySliderConstraint.cpp + Featherstone/btMultiBodyJointMotor.cpp + Featherstone/btMultiBodyGearConstraint.cpp + MLCPSolvers/btDantzigLCP.cpp + MLCPSolvers/btMLCPSolver.cpp + MLCPSolvers/btLemkeAlgorithm.cpp +) + +SET(Root_HDRS + ../btBulletDynamicsCommon.h + ../btBulletCollisionCommon.h +) +SET(ConstraintSolver_HDRS + ConstraintSolver/btConeTwistConstraint.h + ConstraintSolver/btConstraintSolver.h + ConstraintSolver/btContactConstraint.h + ConstraintSolver/btContactSolverInfo.h + ConstraintSolver/btFixedConstraint.h + ConstraintSolver/btGearConstraint.h + ConstraintSolver/btGeneric6DofConstraint.h + ConstraintSolver/btGeneric6DofSpringConstraint.h + ConstraintSolver/btGeneric6DofSpring2Constraint.h + ConstraintSolver/btHinge2Constraint.h + ConstraintSolver/btHingeConstraint.h + ConstraintSolver/btJacobianEntry.h + ConstraintSolver/btPoint2PointConstraint.h + ConstraintSolver/btSequentialImpulseConstraintSolver.h + ConstraintSolver/btSequentialImpulseConstraintSolverMt.h + ConstraintSolver/btNNCGConstraintSolver.h + ConstraintSolver/btSliderConstraint.h + ConstraintSolver/btSolve2LinearConstraint.h + ConstraintSolver/btSolverBody.h + ConstraintSolver/btSolverConstraint.h + ConstraintSolver/btTypedConstraint.h + ConstraintSolver/btUniversalConstraint.h +) +SET(Dynamics_HDRS + Dynamics/btActionInterface.h + Dynamics/btDiscreteDynamicsWorld.h + Dynamics/btDiscreteDynamicsWorldMt.h + Dynamics/btSimulationIslandManagerMt.h + Dynamics/btDynamicsWorld.h + Dynamics/btSimpleDynamicsWorld.h + Dynamics/btRigidBody.h +) +SET(Vehicle_HDRS + Vehicle/btRaycastVehicle.h + Vehicle/btVehicleRaycaster.h + Vehicle/btWheelInfo.h +) + +SET(Featherstone_HDRS + Featherstone/btMultiBody.h + Featherstone/btMultiBodyConstraintSolver.h + Featherstone/btMultiBodyDynamicsWorld.h + Featherstone/btMultiBodyLink.h + Featherstone/btMultiBodyLinkCollider.h + Featherstone/btMultiBodySolverConstraint.h + Featherstone/btMultiBodyConstraint.h + Featherstone/btMultiBodyJointLimitConstraint.h + Featherstone/btMultiBodyConstraint.h + Featherstone/btMultiBodyPoint2Point.h + Featherstone/btMultiBodyFixedConstraint.h + Featherstone/btMultiBodySliderConstraint.h + Featherstone/btMultiBodyJointMotor.h + Featherstone/btMultiBodyGearConstraint.h +) + +SET(MLCPSolvers_HDRS + MLCPSolvers/btDantzigLCP.h + MLCPSolvers/btDantzigSolver.h + MLCPSolvers/btMLCPSolver.h + MLCPSolvers/btMLCPSolverInterface.h + MLCPSolvers/btPATHSolver.h + MLCPSolvers/btSolveProjectedGaussSeidel.h + MLCPSolvers/btLemkeSolver.h + MLCPSolvers/btLemkeAlgorithm.h +) + +SET(Character_HDRS + Character/btCharacterControllerInterface.h + Character/btKinematicCharacterController.h +) + + + +SET(BulletDynamics_HDRS + ${Root_HDRS} + ${ConstraintSolver_HDRS} + ${Dynamics_HDRS} + ${Vehicle_HDRS} + ${Character_HDRS} + ${Featherstone_HDRS} + ${MLCPSolvers_HDRS} +) + + +ADD_LIBRARY(BulletDynamics ${BulletDynamics_SRCS} ${BulletDynamics_HDRS}) +SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + TARGET_LINK_LIBRARIES(BulletDynamics BulletCollision LinearMath) +ENDIF (BUILD_SHARED_LIBS) + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletDynamics DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletDynamics RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN +".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + INSTALL(FILES ../btBulletDynamicsCommon.h +DESTINATION ${INCLUDE_INSTALL_DIR}/BulletDynamics) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(BulletDynamics PROPERTIES PUBLIC_HEADER "${Root_HDRS}") + # Have to list out sub-directories manually: + SET_PROPERTY(SOURCE ${ConstraintSolver_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/ConstraintSolver) + SET_PROPERTY(SOURCE ${Dynamics_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Dynamics) + SET_PROPERTY(SOURCE ${Vehicle_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Vehicle) + SET_PROPERTY(SOURCE ${Character_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Character) + SET_PROPERTY(SOURCE ${Featherstone_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Featherstone) + SET_PROPERTY(SOURCE ${MLCPSolvers_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/MLCPSolvers) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h b/extern/bullet/src/BulletDynamics/Character/btCharacterControllerInterface.h similarity index 96% rename from extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h rename to extern/bullet/src/BulletDynamics/Character/btCharacterControllerInterface.h index dffb06dfe94d..abe24b5ca6cb 100644 --- a/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h +++ b/extern/bullet/src/BulletDynamics/Character/btCharacterControllerInterface.h @@ -37,7 +37,7 @@ class btCharacterControllerInterface : public btActionInterface virtual void preStep ( btCollisionWorld* collisionWorld) = 0; virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0; virtual bool canJump () const = 0; - virtual void jump () = 0; + virtual void jump(const btVector3& dir = btVector3(0, 0, 0)) = 0; virtual bool onGround () const = 0; virtual void setUpInterpolate (bool value) = 0; diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/extern/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp similarity index 64% rename from extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp rename to extern/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp index 31faf1df5e3b..cb1aa71a14e4 100644 --- a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.cpp +++ b/extern/bullet/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -132,30 +132,38 @@ btVector3 btKinematicCharacterController::perpindicularComponent (const btVector return direction - parallelComponent(direction, normal); } -btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis) +btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up) { - m_upAxis = upAxis; - m_addedMargin = 0.02; - m_walkDirection.setValue(0,0,0); - m_useGhostObjectSweepTest = true; m_ghostObject = ghostObject; - m_stepHeight = stepHeight; + m_up.setValue(0.0f, 0.0f, 1.0f); + m_jumpAxis.setValue(0.0f, 0.0f, 1.0f); + m_addedMargin = 0.02; + m_walkDirection.setValue(0.0,0.0,0.0); + m_AngVel.setValue(0.0, 0.0, 0.0); + m_useGhostObjectSweepTest = true; m_turnAngle = btScalar(0.0); m_convexShape=convexShape; m_useWalkDirection = true; // use walk direction by default, legacy behavior m_velocityTimeInterval = 0.0; m_verticalVelocity = 0.0; m_verticalOffset = 0.0; - m_gravity = 9.8 * 3 ; // 3G acceleration. + m_gravity = 9.8 * 3.0 ; // 3G acceleration. m_fallSpeed = 55.0; // Terminal velocity of a sky diver in m/s. m_jumpSpeed = 10.0; // ? + m_SetjumpSpeed = m_jumpSpeed; m_wasOnGround = false; m_wasJumping = false; m_interpolateUp = true; - setMaxSlope(btRadians(45.0)); - m_currentStepOffset = 0; + m_currentStepOffset = 0.0; + m_maxPenetrationDepth = 0.2; full_drop = false; bounce_fix = false; + m_linearDamping = btScalar(0.0); + m_angularDamping = btScalar(0.0); + + setUp(up); + setStepHeight(stepHeight); + setMaxSlope(btRadians(45.0)); } btKinematicCharacterController::~btKinematicCharacterController () @@ -190,7 +198,7 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); - btScalar maxPen = btScalar(0.0); +// btScalar maxPen = btScalar(0.0); for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) { m_manifoldArray.resize(0); @@ -198,10 +206,13 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i]; btCollisionObject* obj0 = static_cast(collisionPair->m_pProxy0->m_clientObject); - btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); + btCollisionObject* obj1 = static_cast(collisionPair->m_pProxy1->m_clientObject); if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse())) continue; + + if (!needsCollision(obj0, obj1)) + continue; if (collisionPair->m_algorithm) collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray); @@ -217,14 +228,15 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* btScalar dist = pt.getDistance(); - if (dist < 0.0) + if (dist < -m_maxPenetrationDepth) { - if (dist < maxPen) - { - maxPen = dist; - m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? + // TODO: cause problems on slopes, not sure if it is needed + //if (dist < maxPen) + //{ + // maxPen = dist; + // m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? - } + //} m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * btScalar(0.2); penetration = true; } else { @@ -244,18 +256,28 @@ bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* void btKinematicCharacterController::stepUp ( btCollisionWorld* world) { + btScalar stepHeight = 0.0f; + if (m_verticalVelocity < 0.0) + stepHeight = m_stepHeight; + // phase 1: up btTransform start, end; - m_targetPosition = m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.f?m_verticalOffset:0.f)); start.setIdentity (); end.setIdentity (); /* FIXME: Handle penetration properly */ - start.setOrigin (m_currentPosition + getUpAxisDirections()[m_upAxis] * (m_convexShape->getMargin() + m_addedMargin)); + start.setOrigin(m_currentPosition); + + m_targetPosition = m_currentPosition + m_up * (stepHeight) + m_jumpAxis * ((m_verticalOffset > 0.f ? m_verticalOffset : 0.f)); + m_currentPosition = m_targetPosition; + end.setOrigin (m_targetPosition); - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, -getUpAxisDirections()[m_upAxis], btScalar(0.7071)); + start.setRotation(m_currentOrientation); + end.setRotation(m_targetOrientation); + + btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, -m_up, m_maxSlopeCosine); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; @@ -265,29 +287,61 @@ void btKinematicCharacterController::stepUp ( btCollisionWorld* world) } else { - world->convexSweepTest (m_convexShape, start, end, callback); + world->convexSweepTest(m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); } - - if (callback.hasHit()) + + if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) { // Only modify the position if the hit was a slope and not a wall or ceiling. - if(callback.m_hitNormalWorld.dot(getUpAxisDirections()[m_upAxis]) > 0.0) + if (callback.m_hitNormalWorld.dot(m_up) > 0.0) { // we moved up only a fraction of the step height - m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction; + m_currentStepOffset = stepHeight * callback.m_closestHitFraction; if (m_interpolateUp == true) m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); else m_currentPosition = m_targetPosition; } - m_verticalVelocity = 0.0; - m_verticalOffset = 0.0; + + btTransform& xform = m_ghostObject->getWorldTransform(); + xform.setOrigin(m_currentPosition); + m_ghostObject->setWorldTransform(xform); + + // fix penetration if we hit a ceiling for example + int numPenetrationLoops = 0; + m_touchingContact = false; + while (recoverFromPenetration(world)) + { + numPenetrationLoops++; + m_touchingContact = true; + if (numPenetrationLoops > 4) + { + //printf("character could not recover from penetration = %d\n", numPenetrationLoops); + break; + } + } + m_targetPosition = m_ghostObject->getWorldTransform().getOrigin(); + m_currentPosition = m_targetPosition; + + if (m_verticalOffset > 0) + { + m_verticalOffset = 0.0; + m_verticalVelocity = 0.0; + m_currentStepOffset = m_stepHeight; + } } else { - m_currentStepOffset = m_stepHeight; + m_currentStepOffset = stepHeight; m_currentPosition = m_targetPosition; } } +bool btKinematicCharacterController::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1) +{ + bool collides = (body0->getBroadphaseHandle()->m_collisionFilterGroup & body1->getBroadphaseHandle()->m_collisionFilterMask) != 0; + collides = collides && (body1->getBroadphaseHandle()->m_collisionFilterGroup & body0->getBroadphaseHandle()->m_collisionFilterMask); + return collides; +} + void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) { btVector3 movementDirection = m_targetPosition - m_currentPosition; @@ -330,6 +384,7 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co // m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]); // phase 2: forward and strafe btTransform start, end; + m_targetPosition = m_currentPosition + walkMove; start.setIdentity (); @@ -339,15 +394,6 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co btScalar distance2 = (m_currentPosition-m_targetPosition).length2(); // printf("distance2=%f\n",distance2); - if (m_touchingContact) - { - if (m_normalizedDirection.dot(m_touchingNormal) > btScalar(0.0)) - { - //interferes with step movement - //updateTargetPositionBasedOnCollision (m_touchingNormal); - } - } - int maxIter = 10; while (fraction > btScalar(0.01) && maxIter-- > 0) @@ -356,6 +402,9 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co end.setOrigin (m_targetPosition); btVector3 sweepDirNegative(m_currentPosition - m_targetPosition); + start.setRotation(m_currentOrientation); + end.setRotation(m_targetOrientation); + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, sweepDirNegative, btScalar(0.0)); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; @@ -364,21 +413,23 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co btScalar margin = m_convexShape->getMargin(); m_convexShape->setMargin(margin + m_addedMargin); - - if (m_useGhostObjectSweepTest) + if (!(start == end)) { - 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 (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()) + if (callback.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject)) { // we moved only a fraction //btScalar hitDistance; @@ -403,14 +454,11 @@ void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* co break; } - } else { - // we moved whole way - m_currentPosition = m_targetPosition; } - - // if (callback.m_closestHitFraction == 0.f) - // break; - + else + { + m_currentPosition = m_targetPosition; + } } } @@ -421,27 +469,30 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld // phase 3: down /*btScalar additionalDownStep = (m_wasOnGround && !onGround()) ? m_stepHeight : 0.0; - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + additionalDownStep); + btVector3 step_drop = m_up * (m_currentStepOffset + additionalDownStep); btScalar downVelocity = (additionalDownStep == 0.0 && m_verticalVelocity<0.0?-m_verticalVelocity:0.0) * dt; - btVector3 gravity_drop = getUpAxisDirections()[m_upAxis] * downVelocity; + btVector3 gravity_drop = m_up * downVelocity; m_targetPosition -= (step_drop + gravity_drop);*/ btVector3 orig_position = m_targetPosition; btScalar downVelocity = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; + if (m_verticalVelocity > 0.0) + return; + if(downVelocity > 0.0 && downVelocity > m_fallSpeed && (m_wasOnGround || !m_wasJumping)) downVelocity = m_fallSpeed; - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + btVector3 step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + btKinematicClosestNotMeConvexResultCallback callback(m_ghostObject, m_up, m_maxSlopeCosine); callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - btKinematicClosestNotMeConvexResultCallback callback2 (m_ghostObject, getUpAxisDirections()[m_upAxis], m_maxSlopeCosine); + btKinematicClosestNotMeConvexResultCallback callback2(m_ghostObject, m_up, m_maxSlopeCosine); callback2.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback2.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; @@ -455,6 +506,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld start.setOrigin (m_currentPosition); end.setOrigin (m_targetPosition); + start.setRotation(m_currentOrientation); + end.setRotation(m_targetOrientation); + //set double test for 2x the step drop, to check for a large drop vs small drop end_double.setOrigin (m_targetPosition - step_drop); @@ -462,7 +516,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - if (!callback.hasHit()) + if (!callback.hasHit() && m_ghostObject->hasContactResponse()) { //test a double fall height, to see if the character should interpolate it's fall (full) or not (partial) m_ghostObject->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); @@ -471,30 +525,34 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - if (!callback.hasHit()) - { - //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) - collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } + if (!callback.hasHit() && m_ghostObject->hasContactResponse()) + { + //test a double fall height, to see if the character should interpolate it's fall (large) or not (small) + collisionWorld->convexSweepTest (m_convexShape, start, end_double, callback2, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } } btScalar downVelocity2 = (m_verticalVelocity<0.f?-m_verticalVelocity:0.f) * dt; - bool has_hit = false; + bool has_hit; if (bounce_fix == true) - has_hit = callback.hasHit() || callback2.hasHit(); + has_hit = (callback.hasHit() || callback2.hasHit()) && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback.m_hitCollisionObject); else - has_hit = callback2.hasHit(); + has_hit = callback2.hasHit() && m_ghostObject->hasContactResponse() && needsCollision(m_ghostObject, callback2.m_hitCollisionObject); - if(downVelocity2 > 0.0 && downVelocity2 < m_stepHeight && has_hit == true && runonce == false + btScalar stepHeight = 0.0f; + if (m_verticalVelocity < 0.0) + stepHeight = m_stepHeight; + + if (downVelocity2 > 0.0 && downVelocity2 < stepHeight && has_hit == true && runonce == false && (m_wasOnGround || !m_wasJumping)) { //redo the velocity calculation when falling a small amount, for fast stairs motion //for larger falls, use the smoother/slower interpolated movement by not touching the target position m_targetPosition = orig_position; - downVelocity = m_stepHeight; + downVelocity = stepHeight; - btVector3 step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; runonce = true; continue; //re-run previous tests @@ -502,10 +560,9 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld break; } - if (callback.hasHit() || runonce == true) + if ((m_ghostObject->hasContactResponse() && (callback.hasHit() && needsCollision(m_ghostObject, callback.m_hitCollisionObject))) || runonce == true) { // we dropped a fraction of the height -> hit floor - btScalar fraction = (m_currentPosition.getY() - callback.m_hitPointWorld.getY()) / 2; //printf("hitpoint: %g - pos %g\n", callback.m_hitPointWorld.getY(), m_currentPosition.getY()); @@ -513,10 +570,10 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld if (bounce_fix == true) { if (full_drop == true) - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); - else - //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction); + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + else + //due to errors in the closestHitFraction variable when used with large polygons, calculate the hit fraction manually + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, fraction); } else m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); @@ -528,7 +585,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld m_wasJumping = false; } else { // we dropped the full height - + full_drop = true; if (bounce_fix == true) @@ -538,7 +595,7 @@ void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld { m_targetPosition += step_drop; //undo previous target change downVelocity = m_fallSpeed; - step_drop = getUpAxisDirections()[m_upAxis] * (m_currentStepOffset + downVelocity); + step_drop = m_up * (m_currentStepOffset + downVelocity); m_targetPosition -= step_drop; } } @@ -579,21 +636,63 @@ btScalar timeInterval m_velocityTimeInterval += timeInterval; } -void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld ) +void btKinematicCharacterController::setAngularVelocity(const btVector3& velocity) { - m_verticalVelocity = 0.0; - m_verticalOffset = 0.0; - m_wasOnGround = false; - m_wasJumping = false; - m_walkDirection.setValue(0,0,0); - m_velocityTimeInterval = 0.0; + m_AngVel = velocity; +} - //clear pair cache - btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache(); - while (cache->getOverlappingPairArray().size() > 0) - { - cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher()); - } +const btVector3& btKinematicCharacterController::getAngularVelocity() const +{ + return m_AngVel; +} + +void btKinematicCharacterController::setLinearVelocity(const btVector3& velocity) +{ + m_walkDirection = velocity; + + // HACK: if we are moving in the direction of the up, treat it as a jump :( + if (m_walkDirection.length2() > 0) + { + btVector3 w = velocity.normalized(); + btScalar c = w.dot(m_up); + if (c != 0) + { + //there is a component in walkdirection for vertical velocity + btVector3 upComponent = m_up * (btSin(SIMD_HALF_PI - btAcos(c)) * m_walkDirection.length()); + m_walkDirection -= upComponent; + m_verticalVelocity = (c < 0.0f ? -1 : 1) * upComponent.length(); + + if (c > 0.0f) + { + m_wasJumping = true; + m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin(); + } + } + } + else + m_verticalVelocity = 0.0f; +} + +btVector3 btKinematicCharacterController::getLinearVelocity() const +{ + return m_walkDirection + (m_verticalVelocity * m_up); +} + +void btKinematicCharacterController::reset ( btCollisionWorld* collisionWorld ) +{ + m_verticalVelocity = 0.0; + m_verticalOffset = 0.0; + m_wasOnGround = false; + m_wasJumping = false; + m_walkDirection.setValue(0,0,0); + m_velocityTimeInterval = 0.0; + + //clear pair cache + btHashedOverlappingPairCache *cache = m_ghostObject->getOverlappingPairCache(); + while (cache->getOverlappingPairArray().size() > 0) + { + cache->removeOverlappingPair(cache->getOverlappingPairArray()[0].m_pProxy0, cache->getOverlappingPairArray()[0].m_pProxy1, collisionWorld->getDispatcher()); + } } void btKinematicCharacterController::warp (const btVector3& origin) @@ -607,62 +706,99 @@ void btKinematicCharacterController::warp (const btVector3& origin) 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]); - + m_currentOrientation = m_ghostObject->getWorldTransform().getRotation(); + m_targetOrientation = m_currentOrientation; +// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]); } -#include - void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt) { // printf("playerStep(): "); // printf(" dt = %f", dt); + if (m_AngVel.length2() > 0.0f) + { + m_AngVel *= btPow(btScalar(1) - m_angularDamping, dt); + } + + // integrate for angular velocity + if (m_AngVel.length2() > 0.0f) + { + btTransform xform; + xform = m_ghostObject->getWorldTransform(); + + btQuaternion rot(m_AngVel.normalized(), m_AngVel.length() * dt); + + btQuaternion orn = rot * xform.getRotation(); + + xform.setRotation(orn); + m_ghostObject->setWorldTransform(xform); + + m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); + m_targetPosition = m_currentPosition; + m_currentOrientation = m_ghostObject->getWorldTransform().getRotation(); + m_targetOrientation = m_currentOrientation; + } + // quick check... - if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) { + if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0)) { // printf("\n"); return; // no motion } m_wasOnGround = onGround(); + //btVector3 lvel = m_walkDirection; + //btScalar c = 0.0f; + + if (m_walkDirection.length2() > 0) + { + // apply damping + m_walkDirection *= btPow(btScalar(1) - m_linearDamping, dt); + } + + m_verticalVelocity *= btPow(btScalar(1) - m_linearDamping, dt); + // Update fall velocity. m_verticalVelocity -= m_gravity * dt; - if(m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) + if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) { m_verticalVelocity = m_jumpSpeed; } - if(m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) + if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) { m_verticalVelocity = -btFabs(m_fallSpeed); } m_verticalOffset = m_verticalVelocity * dt; - btTransform xform; - xform = m_ghostObject->getWorldTransform (); + xform = m_ghostObject->getWorldTransform(); // printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]); // printf("walkSpeed=%f\n",walkSpeed); - stepUp (collisionWorld); + stepUp(collisionWorld); + //todo: Experimenting with behavior of controller when it hits a ceiling.. + //bool hitUp = stepUp (collisionWorld); + //if (hitUp) + //{ + // m_verticalVelocity -= m_gravity * dt; + // if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed) + // { + // m_verticalVelocity = m_jumpSpeed; + // } + // if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed)) + // { + // m_verticalVelocity = -btFabs(m_fallSpeed); + // } + // m_verticalOffset = m_verticalVelocity * dt; + + // xform = m_ghostObject->getWorldTransform(); + //} + if (m_useWalkDirection) { stepForwardAndStrafe (collisionWorld, m_walkDirection); } else { @@ -682,10 +818,38 @@ void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWo } stepDown (collisionWorld, dt); + //todo: Experimenting with max jump height + //if (m_wasJumping) + //{ + // btScalar ds = m_currentPosition[m_upAxis] - m_jumpPosition[m_upAxis]; + // if (ds > m_maxJumpHeight) + // { + // // substract the overshoot + // m_currentPosition[m_upAxis] -= ds - m_maxJumpHeight; + + // // max height was reached, so potential energy is at max + // // and kinematic energy is 0, thus velocity is 0. + // if (m_verticalVelocity > 0.0) + // m_verticalVelocity = 0.0; + // } + //} // printf("\n"); xform.setOrigin (m_currentPosition); m_ghostObject->setWorldTransform (xform); + + 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; + } + } } void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) @@ -696,6 +860,7 @@ void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed) { m_jumpSpeed = jumpSpeed; + m_SetjumpSpeed = m_jumpSpeed; } void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight) @@ -708,14 +873,16 @@ bool btKinematicCharacterController::canJump () const return onGround(); } -void btKinematicCharacterController::jump () +void btKinematicCharacterController::jump(const btVector3& v) { - if (!canJump()) - return; - + m_jumpSpeed = v.length2() == 0 ? m_SetjumpSpeed : v.length(); m_verticalVelocity = m_jumpSpeed; m_wasJumping = true; + m_jumpAxis = v.length2() == 0 ? m_up : v.normalized(); + + m_jumpPosition = m_ghostObject->getWorldTransform().getOrigin(); + #if 0 currently no jumping. btTransform xform; @@ -727,14 +894,16 @@ void btKinematicCharacterController::jump () #endif } -void btKinematicCharacterController::setGravity(btScalar gravity) +void btKinematicCharacterController::setGravity(const btVector3& gravity) { - m_gravity = gravity; + if (gravity.length2() > 0) setUpVector(-gravity); + + m_gravity = gravity.length(); } -btScalar btKinematicCharacterController::getGravity() const +btVector3 btKinematicCharacterController::getGravity() const { - return m_gravity; + return -m_gravity * m_up; } void btKinematicCharacterController::setMaxSlope(btScalar slopeRadians) @@ -748,11 +917,25 @@ btScalar btKinematicCharacterController::getMaxSlope() const return m_maxSlopeRadians; } +void btKinematicCharacterController::setMaxPenetrationDepth(btScalar d) +{ + m_maxPenetrationDepth = d; +} + +btScalar btKinematicCharacterController::getMaxPenetrationDepth() const +{ + return m_maxPenetrationDepth; +} + bool btKinematicCharacterController::onGround () const { - return m_verticalVelocity == 0.0 && m_verticalOffset == 0.0; + return (fabs(m_verticalVelocity) < SIMD_EPSILON) && (fabs(m_verticalOffset) < SIMD_EPSILON); } +void btKinematicCharacterController::setStepHeight(btScalar h) +{ + m_stepHeight = h; +} btVector3* btKinematicCharacterController::getUpAxisDirections() { @@ -769,3 +952,49 @@ void btKinematicCharacterController::setUpInterpolate(bool value) { m_interpolateUp = value; } + +void btKinematicCharacterController::setUp(const btVector3& up) +{ + if (up.length2() > 0 && m_gravity > 0.0f) + { + setGravity(-m_gravity * up.normalized()); + return; + } + + setUpVector(up); +} + +void btKinematicCharacterController::setUpVector(const btVector3& up) +{ + if (m_up == up) + return; + + btVector3 u = m_up; + + if (up.length2() > 0) + m_up = up.normalized(); + else + m_up = btVector3(0.0, 0.0, 0.0); + + if (!m_ghostObject) return; + btQuaternion rot = getRotation(m_up, u); + + //set orientation with new up + btTransform xform; + xform = m_ghostObject->getWorldTransform(); + btQuaternion orn = rot.inverse() * xform.getRotation(); + xform.setRotation(orn); + m_ghostObject->setWorldTransform(xform); +} + +btQuaternion btKinematicCharacterController::getRotation(btVector3& v0, btVector3& v1) const +{ + if (v0.length2() == 0.0f || v1.length2() == 0.0f) + { + btQuaternion q; + return q; + } + + return shortestArcQuatNormalize2(v0, v1); +} + diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h b/extern/bullet/src/BulletDynamics/Character/btKinematicCharacterController.h similarity index 77% rename from extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h rename to extern/bullet/src/BulletDynamics/Character/btKinematicCharacterController.h index add6f30a6872..00c59c02480d 100644 --- a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h +++ b/extern/bullet/src/BulletDynamics/Character/btKinematicCharacterController.h @@ -43,10 +43,12 @@ ATTRIBUTE_ALIGNED16(class) btKinematicCharacterController : public btCharacterCo btPairCachingGhostObject* m_ghostObject; btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast + btScalar m_maxPenetrationDepth; btScalar m_verticalVelocity; btScalar m_verticalOffset; btScalar m_fallSpeed; btScalar m_jumpSpeed; + btScalar m_SetjumpSpeed; btScalar m_maxJumpHeight; btScalar m_maxSlopeRadians; // Slope angle that is set (used for returning the exact value) btScalar m_maxSlopeCosine; // Cosine equivalent of m_maxSlopeRadians (calculated once when set, for optimization) @@ -61,24 +63,34 @@ ATTRIBUTE_ALIGNED16(class) btKinematicCharacterController : public btCharacterCo ///this is the desired walk direction, set by the user btVector3 m_walkDirection; btVector3 m_normalizedDirection; + btVector3 m_AngVel; + + btVector3 m_jumpPosition; //some internal variables btVector3 m_currentPosition; btScalar m_currentStepOffset; btVector3 m_targetPosition; + btQuaternion m_currentOrientation; + btQuaternion m_targetOrientation; + ///keep track of the contact manifolds btManifoldArray m_manifoldArray; bool m_touchingContact; btVector3 m_touchingNormal; + btScalar m_linearDamping; + btScalar m_angularDamping; + bool m_wasOnGround; bool m_wasJumping; bool m_useGhostObjectSweepTest; bool m_useWalkDirection; btScalar m_velocityTimeInterval; - int m_upAxis; + btVector3 m_up; + btVector3 m_jumpAxis; static btVector3* getUpAxisDirections(); bool m_interpolateUp; @@ -94,11 +106,18 @@ ATTRIBUTE_ALIGNED16(class) btKinematicCharacterController : public btCharacterCo void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0)); void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove); void stepDown (btCollisionWorld* collisionWorld, btScalar dt); + + virtual bool needsCollision(const btCollisionObject* body0, const btCollisionObject* body1); + + void setUpVector(const btVector3& up); + + btQuaternion getRotation(btVector3& v0, btVector3& v1) const; + public: BT_DECLARE_ALIGNED_ALLOCATOR(); - btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1); + btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, const btVector3& up = btVector3(1.0,0.0,0.0)); ~btKinematicCharacterController (); @@ -112,14 +131,9 @@ ATTRIBUTE_ALIGNED16(class) btKinematicCharacterController : public btCharacterCo ///btActionInterface interface void debugDraw(btIDebugDraw* debugDrawer); - void setUpAxis (int axis) - { - if (axis < 0) - axis = 0; - if (axis > 2) - axis = 2; - m_upAxis = axis; - } + void setUp(const btVector3& up); + + const btVector3& getUp() { return m_up; } /// This should probably be called setPositionIncrementPerSimulatorStep. /// This is neither a direction nor a velocity, but the amount to @@ -136,27 +150,47 @@ ATTRIBUTE_ALIGNED16(class) btKinematicCharacterController : public btCharacterCo virtual void setVelocityForTimeInterval(const btVector3& velocity, btScalar timeInterval); + virtual void setAngularVelocity(const btVector3& velocity); + virtual const btVector3& getAngularVelocity() const; + + virtual void setLinearVelocity(const btVector3& velocity); + virtual btVector3 getLinearVelocity() const; + + void setLinearDamping(btScalar d) { m_linearDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); } + btScalar getLinearDamping() const { return m_linearDamping; } + void setAngularDamping(btScalar d) { m_angularDamping = btClamped(d, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); } + btScalar getAngularDamping() const { return m_angularDamping; } + void reset ( btCollisionWorld* collisionWorld ); void warp (const btVector3& origin); void preStep ( btCollisionWorld* collisionWorld); void playerStep ( btCollisionWorld* collisionWorld, btScalar dt); + void setStepHeight(btScalar h); + btScalar getStepHeight() const { return m_stepHeight; } void setFallSpeed (btScalar fallSpeed); + btScalar getFallSpeed() const { return m_fallSpeed; } void setJumpSpeed (btScalar jumpSpeed); + btScalar getJumpSpeed() const { return m_jumpSpeed; } void setMaxJumpHeight (btScalar maxJumpHeight); bool canJump () const; - void jump (); + void jump(const btVector3& v = btVector3(0, 0, 0)); + + void applyImpulse(const btVector3& v) { jump(v); } - void setGravity(btScalar gravity); - btScalar getGravity() const; + void setGravity(const btVector3& gravity); + btVector3 getGravity() const; /// The max slope determines the maximum angle that the controller can walk up. /// The slope angle is measured in radians. void setMaxSlope(btScalar slopeRadians); btScalar getMaxSlope() const; + void setMaxPenetrationDepth(btScalar d); + btScalar getMaxPenetrationDepth() const; + btPairCachingGhostObject* getGhostObject(); void setUseGhostSweepTest(bool useGhostObjectSweepTest) { diff --git a/extern/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp new file mode 100644 index 000000000000..c82ba87f9ff8 --- /dev/null +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.cpp @@ -0,0 +1,1128 @@ +/* +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 "btBatchedConstraints.h" + +#include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btMinMax.h" +#include "LinearMath/btStackAlloc.h" +#include "LinearMath/btQuickprof.h" + +#include //for memset + +const int kNoMerge = -1; + +bool btBatchedConstraints::s_debugDrawBatches = false; + + +struct btBatchedConstraintInfo +{ + int constraintIndex; + int numConstraintRows; + int bodyIds[2]; +}; + + +struct btBatchInfo +{ + int numConstraints; + int mergeIndex; + + btBatchInfo() : numConstraints(0), mergeIndex(kNoMerge) {} +}; + + +bool btBatchedConstraints::validate(btConstraintArray* constraints, const btAlignedObjectArray& bodies) const +{ + // + // validate: for debugging only. Verify coloring of bodies, that no body is touched by more than one batch in any given phase + // + int errors = 0; + const int kUnassignedBatch = -1; + + btAlignedObjectArray bodyBatchId; + for (int iPhase = 0; iPhase < m_phases.size(); ++iPhase) + { + bodyBatchId.resizeNoInitialize(0); + bodyBatchId.resize( bodies.size(), kUnassignedBatch ); + const Range& phase = m_phases[iPhase]; + for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch) + { + const Range& batch = m_batches[iBatch]; + for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons) + { + int iCons = m_constraintIndices[iiCons]; + const btSolverConstraint& cons = constraints->at(iCons); + const btSolverBody& bodyA = bodies[cons.m_solverBodyIdA]; + const btSolverBody& bodyB = bodies[cons.m_solverBodyIdB]; + if (! bodyA.internalGetInvMass().isZero()) + { + int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdA]; + if (thisBodyBatchId == kUnassignedBatch) + { + bodyBatchId[cons.m_solverBodyIdA] = iBatch; + } + else if (thisBodyBatchId != iBatch) + { + btAssert( !"dynamic body is used in 2 different batches in the same phase" ); + errors++; + } + } + if (! bodyB.internalGetInvMass().isZero()) + { + int thisBodyBatchId = bodyBatchId[cons.m_solverBodyIdB]; + if (thisBodyBatchId == kUnassignedBatch) + { + bodyBatchId[cons.m_solverBodyIdB] = iBatch; + } + else if (thisBodyBatchId != iBatch) + { + btAssert( !"dynamic body is used in 2 different batches in the same phase" ); + errors++; + } + } + } + } + } + return errors == 0; +} + + +static void debugDrawSingleBatch( const btBatchedConstraints* bc, + btConstraintArray* constraints, + const btAlignedObjectArray& bodies, + int iBatch, + const btVector3& color, + const btVector3& offset + ) +{ + if (bc && bc->m_debugDrawer && iBatch < bc->m_batches.size()) + { + const btBatchedConstraints::Range& b = bc->m_batches[iBatch]; + for (int iiCon = b.begin; iiCon < b.end; ++iiCon) + { + int iCon = bc->m_constraintIndices[iiCon]; + const btSolverConstraint& con = constraints->at(iCon); + int iBody0 = con.m_solverBodyIdA; + int iBody1 = con.m_solverBodyIdB; + btVector3 pos0 = bodies[iBody0].getWorldTransform().getOrigin() + offset; + btVector3 pos1 = bodies[iBody1].getWorldTransform().getOrigin() + offset; + bc->m_debugDrawer->drawLine(pos0, pos1, color); + } + } +} + + +static void debugDrawPhase( const btBatchedConstraints* bc, + btConstraintArray* constraints, + const btAlignedObjectArray& bodies, + int iPhase, + const btVector3& color0, + const btVector3& color1, + const btVector3& offset + ) +{ + BT_PROFILE( "debugDrawPhase" ); + if ( bc && bc->m_debugDrawer && iPhase < bc->m_phases.size() ) + { + const btBatchedConstraints::Range& phase = bc->m_phases[iPhase]; + for (int iBatch = phase.begin; iBatch < phase.end; ++iBatch) + { + float tt = float(iBatch - phase.begin) / float(btMax(1, phase.end - phase.begin - 1)); + btVector3 col = lerp(color0, color1, tt); + debugDrawSingleBatch(bc, constraints, bodies, iBatch, col, offset); + } + } +} + + +static void debugDrawAllBatches( const btBatchedConstraints* bc, + btConstraintArray* constraints, + const btAlignedObjectArray& bodies + ) +{ + BT_PROFILE( "debugDrawAllBatches" ); + if ( bc && bc->m_debugDrawer && bc->m_phases.size() > 0 ) + { + btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT); + btVector3 bboxMax = -bboxMin; + for (int iBody = 0; iBody < bodies.size(); ++iBody) + { + const btVector3& pos = bodies[iBody].getWorldTransform().getOrigin(); + bboxMin.setMin(pos); + bboxMax.setMax(pos); + } + btVector3 bboxExtent = bboxMax - bboxMin; + btVector3 offsetBase = btVector3( 0, bboxExtent.y()*1.1f, 0 ); + btVector3 offsetStep = btVector3( 0, 0, bboxExtent.z()*1.1f ); + int numPhases = bc->m_phases.size(); + for (int iPhase = 0; iPhase < numPhases; ++iPhase) + { + float b = float(iPhase)/float(numPhases-1); + btVector3 color0 = btVector3(1,0,b); + btVector3 color1 = btVector3(0,1,b); + btVector3 offset = offsetBase + offsetStep*(float(iPhase) - float(numPhases-1)*0.5); + debugDrawPhase(bc, constraints, bodies, iPhase, color0, color1, offset); + } + } +} + + +static void initBatchedBodyDynamicFlags(btAlignedObjectArray* outBodyDynamicFlags, const btAlignedObjectArray& bodies) +{ + BT_PROFILE("initBatchedBodyDynamicFlags"); + btAlignedObjectArray& bodyDynamicFlags = *outBodyDynamicFlags; + bodyDynamicFlags.resizeNoInitialize(bodies.size()); + for (int i = 0; i < bodies.size(); ++i) + { + const btSolverBody& body = bodies[ i ]; + bodyDynamicFlags[i] = ( body.internalGetInvMass().x() > btScalar( 0 ) ); + } +} + + +static int runLengthEncodeConstraintInfo(btBatchedConstraintInfo* outConInfos, int numConstraints) +{ + BT_PROFILE("runLengthEncodeConstraintInfo"); + // detect and run-length encode constraint rows that repeat the same bodies + int iDest = 0; + int iSrc = 0; + while (iSrc < numConstraints) + { + const btBatchedConstraintInfo& srcConInfo = outConInfos[iSrc]; + btBatchedConstraintInfo& conInfo = outConInfos[iDest]; + conInfo.constraintIndex = iSrc; + conInfo.bodyIds[0] = srcConInfo.bodyIds[0]; + conInfo.bodyIds[1] = srcConInfo.bodyIds[1]; + while (iSrc < numConstraints && outConInfos[iSrc].bodyIds[0] == srcConInfo.bodyIds[0] && outConInfos[iSrc].bodyIds[1] == srcConInfo.bodyIds[1]) + { + ++iSrc; + } + conInfo.numConstraintRows = iSrc - conInfo.constraintIndex; + ++iDest; + } + return iDest; +} + + +struct ReadSolverConstraintsLoop : public btIParallelForBody +{ + btBatchedConstraintInfo* m_outConInfos; + btConstraintArray* m_constraints; + + ReadSolverConstraintsLoop( btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints ) + { + m_outConInfos = outConInfos; + m_constraints = constraints; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + for (int i = iBegin; i < iEnd; ++i) + { + btBatchedConstraintInfo& conInfo = m_outConInfos[i]; + const btSolverConstraint& con = m_constraints->at( i ); + conInfo.bodyIds[0] = con.m_solverBodyIdA; + conInfo.bodyIds[1] = con.m_solverBodyIdB; + conInfo.constraintIndex = i; + conInfo.numConstraintRows = 1; + } + } +}; + + +static int initBatchedConstraintInfo(btBatchedConstraintInfo* outConInfos, btConstraintArray* constraints) +{ + BT_PROFILE("initBatchedConstraintInfo"); + int numConstraints = constraints->size(); + bool inParallel = true; + if (inParallel) + { + ReadSolverConstraintsLoop loop(outConInfos, constraints); + int grainSize = 1200; + btParallelFor(0, numConstraints, grainSize, loop); + } + else + { + for (int i = 0; i < numConstraints; ++i) + { + btBatchedConstraintInfo& conInfo = outConInfos[i]; + const btSolverConstraint& con = constraints->at( i ); + conInfo.bodyIds[0] = con.m_solverBodyIdA; + conInfo.bodyIds[1] = con.m_solverBodyIdB; + conInfo.constraintIndex = i; + conInfo.numConstraintRows = 1; + } + } + bool useRunLengthEncoding = true; + if (useRunLengthEncoding) + { + numConstraints = runLengthEncodeConstraintInfo(outConInfos, numConstraints); + } + return numConstraints; +} + + +static void expandConstraintRowsInPlace(int* constraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows) +{ + BT_PROFILE("expandConstraintRowsInPlace"); + if (numConstraintRows > numConstraints) + { + // we walk the array in reverse to avoid overwriteing + for (int iCon = numConstraints - 1; iCon >= 0; --iCon) + { + const btBatchedConstraintInfo& conInfo = conInfos[iCon]; + int iBatch = constraintBatchIds[iCon]; + for (int i = conInfo.numConstraintRows - 1; i >= 0; --i) + { + int iDest = conInfo.constraintIndex + i; + btAssert(iDest >= iCon); + btAssert(iDest >= 0 && iDest < numConstraintRows); + constraintBatchIds[iDest] = iBatch; + } + } + } +} + + +static void expandConstraintRows(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows) +{ + BT_PROFILE("expandConstraintRows"); + for ( int iCon = 0; iCon < numConstraints; ++iCon ) + { + const btBatchedConstraintInfo& conInfo = conInfos[ iCon ]; + int iBatch = srcConstraintBatchIds[ iCon ]; + for ( int i = 0; i < conInfo.numConstraintRows; ++i ) + { + int iDest = conInfo.constraintIndex + i; + btAssert( iDest >= iCon ); + btAssert( iDest >= 0 && iDest < numConstraintRows ); + destConstraintBatchIds[ iDest ] = iBatch; + } + } +} + + +struct ExpandConstraintRowsLoop : public btIParallelForBody +{ + int* m_destConstraintBatchIds; + const int* m_srcConstraintBatchIds; + const btBatchedConstraintInfo* m_conInfos; + int m_numConstraintRows; + + ExpandConstraintRowsLoop( int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraintRows) + { + m_destConstraintBatchIds = destConstraintBatchIds; + m_srcConstraintBatchIds = srcConstraintBatchIds; + m_conInfos = conInfos; + m_numConstraintRows = numConstraintRows; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + expandConstraintRows(m_destConstraintBatchIds, m_srcConstraintBatchIds + iBegin, m_conInfos + iBegin, iEnd - iBegin, m_numConstraintRows); + } +}; + + +static void expandConstraintRowsMt(int* destConstraintBatchIds, const int* srcConstraintBatchIds, const btBatchedConstraintInfo* conInfos, int numConstraints, int numConstraintRows) +{ + BT_PROFILE("expandConstraintRowsMt"); + ExpandConstraintRowsLoop loop(destConstraintBatchIds, srcConstraintBatchIds, conInfos, numConstraintRows); + int grainSize = 600; + btParallelFor(0, numConstraints, grainSize, loop); +} + + +static void initBatchedConstraintInfoArray(btAlignedObjectArray* outConInfos, btConstraintArray* constraints) +{ + BT_PROFILE("initBatchedConstraintInfoArray"); + btAlignedObjectArray& conInfos = *outConInfos; + int numConstraints = constraints->size(); + conInfos.resizeNoInitialize(numConstraints); + + int newSize = initBatchedConstraintInfo(&outConInfos->at(0), constraints); + conInfos.resizeNoInitialize(newSize); +} + + +static void mergeSmallBatches(btBatchInfo* batches, int iBeginBatch, int iEndBatch, int minBatchSize, int maxBatchSize) +{ + BT_PROFILE("mergeSmallBatches"); + for ( int iBatch = iEndBatch - 1; iBatch >= iBeginBatch; --iBatch ) + { + btBatchInfo& batch = batches[ iBatch ]; + if ( batch.mergeIndex == kNoMerge && batch.numConstraints > 0 && batch.numConstraints < minBatchSize ) + { + for ( int iDestBatch = iBatch - 1; iDestBatch >= iBeginBatch; --iDestBatch ) + { + btBatchInfo& destBatch = batches[ iDestBatch ]; + if ( destBatch.mergeIndex == kNoMerge && ( destBatch.numConstraints + batch.numConstraints ) < maxBatchSize ) + { + destBatch.numConstraints += batch.numConstraints; + batch.numConstraints = 0; + batch.mergeIndex = iDestBatch; + break; + } + } + } + } + // flatten mergeIndexes + // e.g. in case where A was merged into B and then B was merged into C, we need A to point to C instead of B + // Note: loop goes forward through batches because batches always merge from higher indexes to lower, + // so by going from low to high it reduces the amount of trail-following + for ( int iBatch = iBeginBatch; iBatch < iEndBatch; ++iBatch ) + { + btBatchInfo& batch = batches[ iBatch ]; + if ( batch.mergeIndex != kNoMerge ) + { + int iMergeDest = batches[ batch.mergeIndex ].mergeIndex; + // follow trail of merges to the end + while ( iMergeDest != kNoMerge ) + { + int iNext = batches[ iMergeDest ].mergeIndex; + if ( iNext == kNoMerge ) + { + batch.mergeIndex = iMergeDest; + break; + } + iMergeDest = iNext; + } + } + } +} + + +static void updateConstraintBatchIdsForMerges(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches) +{ + BT_PROFILE("updateConstraintBatchIdsForMerges"); + // update batchIds to account for merges + for (int i = 0; i < numConstraints; ++i) + { + int iBatch = constraintBatchIds[i]; + btAssert(iBatch < numBatches); + // if this constraint references a batch that was merged into another batch + if (batches[iBatch].mergeIndex != kNoMerge) + { + // update batchId + constraintBatchIds[i] = batches[iBatch].mergeIndex; + } + } +} + + +struct UpdateConstraintBatchIdsForMergesLoop : public btIParallelForBody +{ + int* m_constraintBatchIds; + const btBatchInfo* m_batches; + int m_numBatches; + + UpdateConstraintBatchIdsForMergesLoop( int* constraintBatchIds, const btBatchInfo* batches, int numBatches ) + { + m_constraintBatchIds = constraintBatchIds; + m_batches = batches; + m_numBatches = numBatches; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "UpdateConstraintBatchIdsForMergesLoop" ); + updateConstraintBatchIdsForMerges( m_constraintBatchIds + iBegin, iEnd - iBegin, m_batches, m_numBatches ); + } +}; + + +static void updateConstraintBatchIdsForMergesMt(int* constraintBatchIds, int numConstraints, const btBatchInfo* batches, int numBatches) +{ + BT_PROFILE( "updateConstraintBatchIdsForMergesMt" ); + UpdateConstraintBatchIdsForMergesLoop loop(constraintBatchIds, batches, numBatches); + int grainSize = 800; + btParallelFor(0, numConstraints, grainSize, loop); +} + + +inline bool BatchCompare(const btBatchedConstraints::Range& a, const btBatchedConstraints::Range& b) +{ + int lenA = a.end - a.begin; + int lenB = b.end - b.begin; + return lenA > lenB; +} + + +static void writeOutConstraintIndicesForRangeOfBatches(btBatchedConstraints* bc, + const int* constraintBatchIds, + int numConstraints, + int* constraintIdPerBatch, + int batchBegin, + int batchEnd + ) +{ + BT_PROFILE("writeOutConstraintIndicesForRangeOfBatches"); + for ( int iCon = 0; iCon < numConstraints; ++iCon ) + { + int iBatch = constraintBatchIds[ iCon ]; + if (iBatch >= batchBegin && iBatch < batchEnd) + { + int iDestCon = constraintIdPerBatch[ iBatch ]; + constraintIdPerBatch[ iBatch ] = iDestCon + 1; + bc->m_constraintIndices[ iDestCon ] = iCon; + } + } +} + + +struct WriteOutConstraintIndicesLoop : public btIParallelForBody +{ + btBatchedConstraints* m_batchedConstraints; + const int* m_constraintBatchIds; + int m_numConstraints; + int* m_constraintIdPerBatch; + int m_maxNumBatchesPerPhase; + + WriteOutConstraintIndicesLoop( btBatchedConstraints* bc, const int* constraintBatchIds, int numConstraints, int* constraintIdPerBatch, int maxNumBatchesPerPhase ) + { + m_batchedConstraints = bc; + m_constraintBatchIds = constraintBatchIds; + m_numConstraints = numConstraints; + m_constraintIdPerBatch = constraintIdPerBatch; + m_maxNumBatchesPerPhase = maxNumBatchesPerPhase; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "WriteOutConstraintIndicesLoop" ); + int batchBegin = iBegin * m_maxNumBatchesPerPhase; + int batchEnd = iEnd * m_maxNumBatchesPerPhase; + writeOutConstraintIndicesForRangeOfBatches(m_batchedConstraints, + m_constraintBatchIds, + m_numConstraints, + m_constraintIdPerBatch, + batchBegin, + batchEnd + ); + } +}; + + +static void writeOutConstraintIndicesMt(btBatchedConstraints* bc, + const int* constraintBatchIds, + int numConstraints, + int* constraintIdPerBatch, + int maxNumBatchesPerPhase, + int numPhases + ) +{ + BT_PROFILE("writeOutConstraintIndicesMt"); + bool inParallel = true; + if (inParallel) + { + WriteOutConstraintIndicesLoop loop( bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase ); + btParallelFor( 0, numPhases, 1, loop ); + } + else + { + for ( int iCon = 0; iCon < numConstraints; ++iCon ) + { + int iBatch = constraintBatchIds[ iCon ]; + int iDestCon = constraintIdPerBatch[ iBatch ]; + constraintIdPerBatch[ iBatch ] = iDestCon + 1; + bc->m_constraintIndices[ iDestCon ] = iCon; + } + } +} + + +static void writeGrainSizes(btBatchedConstraints* bc) +{ + typedef btBatchedConstraints::Range Range; + int numPhases = bc->m_phases.size(); + bc->m_phaseGrainSize.resizeNoInitialize(numPhases); + int numThreads = btGetTaskScheduler()->getNumThreads(); + for (int iPhase = 0; iPhase < numPhases; ++iPhase) + { + const Range& phase = bc->m_phases[ iPhase ]; + int numBatches = phase.end - phase.begin; + float grainSize = floor((0.25f*numBatches / float(numThreads)) + 0.0f); + bc->m_phaseGrainSize[ iPhase ] = btMax(1, int(grainSize)); + } +} + + +static void writeOutBatches(btBatchedConstraints* bc, + const int* constraintBatchIds, + int numConstraints, + const btBatchInfo* batches, + int* batchWork, + int maxNumBatchesPerPhase, + int numPhases +) +{ + BT_PROFILE("writeOutBatches"); + typedef btBatchedConstraints::Range Range; + bc->m_constraintIndices.reserve( numConstraints ); + bc->m_batches.resizeNoInitialize( 0 ); + bc->m_phases.resizeNoInitialize( 0 ); + + //int maxNumBatches = numPhases * maxNumBatchesPerPhase; + { + int* constraintIdPerBatch = batchWork; // for each batch, keep an index into the next available slot in the m_constraintIndices array + int iConstraint = 0; + for (int iPhase = 0; iPhase < numPhases; ++iPhase) + { + int curPhaseBegin = bc->m_batches.size(); + int iBegin = iPhase * maxNumBatchesPerPhase; + int iEnd = iBegin + maxNumBatchesPerPhase; + for ( int i = iBegin; i < iEnd; ++i ) + { + const btBatchInfo& batch = batches[ i ]; + int curBatchBegin = iConstraint; + constraintIdPerBatch[ i ] = curBatchBegin; // record the start of each batch in m_constraintIndices array + int numConstraints = batch.numConstraints; + iConstraint += numConstraints; + if ( numConstraints > 0 ) + { + bc->m_batches.push_back( Range( curBatchBegin, iConstraint ) ); + } + } + // if any batches were emitted this phase, + if ( bc->m_batches.size() > curPhaseBegin ) + { + // output phase + bc->m_phases.push_back( Range( curPhaseBegin, bc->m_batches.size() ) ); + } + } + + btAssert(iConstraint == numConstraints); + bc->m_constraintIndices.resizeNoInitialize( numConstraints ); + writeOutConstraintIndicesMt( bc, constraintBatchIds, numConstraints, constraintIdPerBatch, maxNumBatchesPerPhase, numPhases ); + } + // for each phase + for (int iPhase = 0; iPhase < bc->m_phases.size(); ++iPhase) + { + // sort the batches from largest to smallest (can be helpful to some task schedulers) + const Range& curBatches = bc->m_phases[iPhase]; + bc->m_batches.quickSortInternal(BatchCompare, curBatches.begin, curBatches.end-1); + } + bc->m_phaseOrder.resize(bc->m_phases.size()); + for (int i = 0; i < bc->m_phases.size(); ++i) + { + bc->m_phaseOrder[i] = i; + } + writeGrainSizes(bc); +} + + +// +// PreallocatedMemoryHelper -- helper object for allocating a number of chunks of memory in a single contiguous block. +// It is generally more efficient to do a single larger allocation than many smaller allocations. +// +// Example Usage: +// +// btVector3* bodyPositions = NULL; +// btBatchedConstraintInfo* conInfos = NULL; +// { +// PreallocatedMemoryHelper<8> memHelper; +// memHelper.addChunk( (void**) &bodyPositions, sizeof( btVector3 ) * bodies.size() ); +// memHelper.addChunk( (void**) &conInfos, sizeof( btBatchedConstraintInfo ) * numConstraints ); +// void* memPtr = malloc( memHelper.getSizeToAllocate() ); // allocate the memory +// memHelper.setChunkPointers( memPtr ); // update pointers to chunks +// } +template +class PreallocatedMemoryHelper +{ + struct Chunk + { + void** ptr; + size_t size; + }; + Chunk m_chunks[N]; + int m_numChunks; +public: + PreallocatedMemoryHelper() {m_numChunks=0;} + void addChunk( void** ptr, size_t sz ) + { + btAssert( m_numChunks < N ); + if ( m_numChunks < N ) + { + Chunk& chunk = m_chunks[ m_numChunks ]; + chunk.ptr = ptr; + chunk.size = sz; + m_numChunks++; + } + } + size_t getSizeToAllocate() const + { + size_t totalSize = 0; + for (int i = 0; i < m_numChunks; ++i) + { + totalSize += m_chunks[i].size; + } + return totalSize; + } + void setChunkPointers(void* mem) const + { + size_t totalSize = 0; + for (int i = 0; i < m_numChunks; ++i) + { + const Chunk& chunk = m_chunks[ i ]; + char* chunkPtr = static_cast(mem) + totalSize; + *chunk.ptr = chunkPtr; + totalSize += chunk.size; + } + } +}; + + + +static btVector3 findMaxDynamicConstraintExtent( + btVector3* bodyPositions, + bool* bodyDynamicFlags, + btBatchedConstraintInfo* conInfos, + int numConstraints, + int numBodies + ) +{ + BT_PROFILE("findMaxDynamicConstraintExtent"); + btVector3 consExtent = btVector3(1,1,1) * 0.001; + for (int iCon = 0; iCon < numConstraints; ++iCon) + { + const btBatchedConstraintInfo& con = conInfos[ iCon ]; + int iBody0 = con.bodyIds[0]; + int iBody1 = con.bodyIds[1]; + btAssert(iBody0 >= 0 && iBody0 < numBodies); + btAssert(iBody1 >= 0 && iBody1 < numBodies); + // is it a dynamic constraint? + if (bodyDynamicFlags[iBody0] && bodyDynamicFlags[iBody1]) + { + btVector3 delta = bodyPositions[iBody1] - bodyPositions[iBody0]; + consExtent.setMax(delta.absolute()); + } + } + return consExtent; +} + + +struct btIntVec3 +{ + int m_ints[ 3 ]; + + SIMD_FORCE_INLINE const int& operator[](int i) const {return m_ints[i];} + SIMD_FORCE_INLINE int& operator[](int i) {return m_ints[i];} +}; + + +struct AssignConstraintsToGridBatchesParams +{ + bool* bodyDynamicFlags; + btIntVec3* bodyGridCoords; + int numBodies; + btBatchedConstraintInfo* conInfos; + int* constraintBatchIds; + btIntVec3 gridChunkDim; + int maxNumBatchesPerPhase; + int numPhases; + int phaseMask; + + AssignConstraintsToGridBatchesParams() + { + memset(this, 0, sizeof(*this)); + } +}; + + +static void assignConstraintsToGridBatches(const AssignConstraintsToGridBatchesParams& params, int iConBegin, int iConEnd) +{ + BT_PROFILE("assignConstraintsToGridBatches"); + // (can be done in parallel) + for ( int iCon = iConBegin; iCon < iConEnd; ++iCon ) + { + const btBatchedConstraintInfo& con = params.conInfos[ iCon ]; + int iBody0 = con.bodyIds[ 0 ]; + int iBody1 = con.bodyIds[ 1 ]; + int iPhase = iCon; //iBody0; // pseudorandom choice to distribute evenly amongst phases + iPhase &= params.phaseMask; + int gridCoord[ 3 ]; + // is it a dynamic constraint? + if ( params.bodyDynamicFlags[ iBody0 ] && params.bodyDynamicFlags[ iBody1 ] ) + { + const btIntVec3& body0Coords = params.bodyGridCoords[iBody0]; + const btIntVec3& body1Coords = params.bodyGridCoords[iBody1]; + // for each dimension x,y,z, + for (int i = 0; i < 3; ++i) + { + int coordMin = btMin(body0Coords.m_ints[i], body1Coords.m_ints[i]); + int coordMax = btMax(body0Coords.m_ints[i], body1Coords.m_ints[i]); + if (coordMin != coordMax) + { + btAssert( coordMax == coordMin + 1 ); + if ((coordMin&1) == 0) + { + iPhase &= ~(1 << i); // force bit off + } + else + { + iPhase |= (1 << i); // force bit on + iPhase &= params.phaseMask; + } + } + gridCoord[ i ] = coordMin; + } + } + else + { + if ( !params.bodyDynamicFlags[ iBody0 ] ) + { + iBody0 = con.bodyIds[ 1 ]; + } + btAssert(params.bodyDynamicFlags[ iBody0 ]); + const btIntVec3& body0Coords = params.bodyGridCoords[iBody0]; + // for each dimension x,y,z, + for ( int i = 0; i < 3; ++i ) + { + gridCoord[ i ] = body0Coords.m_ints[ i ]; + } + } + // calculate chunk coordinates + int chunkCoord[ 3 ]; + btIntVec3 gridChunkDim = params.gridChunkDim; + // for each dimension x,y,z, + for ( int i = 0; i < 3; ++i ) + { + int coordOffset = ( iPhase >> i ) & 1; + chunkCoord[ i ] = (gridCoord[ i ] - coordOffset)/2; + btClamp( chunkCoord[ i ], 0, gridChunkDim[ i ] - 1); + btAssert( chunkCoord[ i ] < gridChunkDim[ i ] ); + } + int iBatch = iPhase * params.maxNumBatchesPerPhase + chunkCoord[ 0 ] + chunkCoord[ 1 ] * gridChunkDim[ 0 ] + chunkCoord[ 2 ] * gridChunkDim[ 0 ] * gridChunkDim[ 1 ]; + btAssert(iBatch >= 0 && iBatch < params.maxNumBatchesPerPhase*params.numPhases); + params.constraintBatchIds[ iCon ] = iBatch; + } +} + + +struct AssignConstraintsToGridBatchesLoop : public btIParallelForBody +{ + const AssignConstraintsToGridBatchesParams* m_params; + + AssignConstraintsToGridBatchesLoop( const AssignConstraintsToGridBatchesParams& params ) + { + m_params = ¶ms; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + assignConstraintsToGridBatches(*m_params, iBegin, iEnd); + } +}; + + +// +// setupSpatialGridBatchesMt -- generate batches using a uniform 3D grid +// +/* + +Bodies are treated as 3D points at their center of mass. We only consider dynamic bodies at this stage, +because only dynamic bodies are mutated when a constraint is solved, thus subject to race conditions. + +1. Compute a bounding box around all dynamic bodies +2. Compute the maximum extent of all dynamic constraints. Each dynamic constraint is treated as a line segment, and we need the size of + box that will fully enclose any single dynamic constraint + +3. Establish the cell size of our grid, the cell size in each dimension must be at least as large as the dynamic constraints max-extent, + so that no dynamic constraint can span more than 2 cells of our grid on any axis of the grid. The cell size should be adjusted + larger in order to keep the total number of cells from being excessively high + +Key idea: Given that each constraint spans 1 or 2 grid cells in each dimension, we can handle all constraints by processing + in chunks of 2x2x2 cells with 8 different 1-cell offsets ((0,0,0),(0,0,1),(0,1,0),(0,1,1),(1,0,0)...). + For each of the 8 offsets, we create a phase, and for each 2x2x2 chunk with dynamic constraints becomes a batch in that phase. + +4. Once the grid is established, we can calculate for each constraint which phase and batch it belongs in. + +5. Do a merge small batches on the batches of each phase separately, to try to even out the sizes of batches + +Optionally, we can "collapse" one dimension of our 3D grid to turn it into a 2D grid, which reduces the number of phases +to 4. With fewer phases, there are more constraints per phase and this makes it easier to create batches of a useful size. +*/ +// +static void setupSpatialGridBatchesMt( + btBatchedConstraints* batchedConstraints, + btAlignedObjectArray* scratchMemory, + btConstraintArray* constraints, + const btAlignedObjectArray& bodies, + int minBatchSize, + int maxBatchSize, + bool use2DGrid +) +{ + BT_PROFILE("setupSpatialGridBatchesMt"); + const int numPhases = 8; + int numConstraints = constraints->size(); + int numConstraintRows = constraints->size(); + + const int maxGridChunkCount = 128; + int allocNumBatchesPerPhase = maxGridChunkCount; + int minNumBatchesPerPhase = 16; + int allocNumBatches = allocNumBatchesPerPhase * numPhases; + + btVector3* bodyPositions = NULL; + bool* bodyDynamicFlags = NULL; + btIntVec3* bodyGridCoords = NULL; + btBatchInfo* batches = NULL; + int* batchWork = NULL; + btBatchedConstraintInfo* conInfos = NULL; + int* constraintBatchIds = NULL; + int* constraintRowBatchIds = NULL; + { + PreallocatedMemoryHelper<10> memHelper; + memHelper.addChunk( (void**) &bodyPositions, sizeof( btVector3 ) * bodies.size() ); + memHelper.addChunk( (void**) &bodyDynamicFlags, sizeof( bool ) * bodies.size() ); + memHelper.addChunk( (void**) &bodyGridCoords, sizeof( btIntVec3 ) * bodies.size() ); + memHelper.addChunk( (void**) &batches, sizeof( btBatchInfo )* allocNumBatches ); + memHelper.addChunk( (void**) &batchWork, sizeof( int )* allocNumBatches ); + memHelper.addChunk( (void**) &conInfos, sizeof( btBatchedConstraintInfo ) * numConstraints ); + memHelper.addChunk( (void**) &constraintBatchIds, sizeof( int ) * numConstraints ); + memHelper.addChunk( (void**) &constraintRowBatchIds, sizeof( int ) * numConstraintRows ); + size_t scratchSize = memHelper.getSizeToAllocate(); + // if we need to reallocate + if (scratchMemory->capacity() < scratchSize) + { + // allocate 6.25% extra to avoid repeated reallocs + scratchMemory->reserve( scratchSize + scratchSize/16 ); + } + scratchMemory->resizeNoInitialize( scratchSize ); + char* memPtr = &scratchMemory->at(0); + memHelper.setChunkPointers( memPtr ); + } + + numConstraints = initBatchedConstraintInfo(conInfos, constraints); + + // compute bounding box around all dynamic bodies + // (could be done in parallel) + btVector3 bboxMin(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT); + btVector3 bboxMax = -bboxMin; + //int dynamicBodyCount = 0; + for (int i = 0; i < bodies.size(); ++i) + { + const btSolverBody& body = bodies[i]; + btVector3 bodyPos = body.getWorldTransform().getOrigin(); + bool isDynamic = ( body.internalGetInvMass().x() > btScalar( 0 ) ); + bodyPositions[i] = bodyPos; + bodyDynamicFlags[i] = isDynamic; + if (isDynamic) + { + //dynamicBodyCount++; + bboxMin.setMin(bodyPos); + bboxMax.setMax(bodyPos); + } + } + + // find max extent of all dynamic constraints + // (could be done in parallel) + btVector3 consExtent = findMaxDynamicConstraintExtent(bodyPositions, bodyDynamicFlags, conInfos, numConstraints, bodies.size()); + + btVector3 gridExtent = bboxMax - bboxMin; + + btVector3 gridCellSize = consExtent; + int gridDim[3]; + gridDim[ 0 ] = int( 1.0 + gridExtent.x() / gridCellSize.x() ); + gridDim[ 1 ] = int( 1.0 + gridExtent.y() / gridCellSize.y() ); + gridDim[ 2 ] = int( 1.0 + gridExtent.z() / gridCellSize.z() ); + + // if we can collapse an axis, it will cut our number of phases in half which could be more efficient + int phaseMask = 7; + bool collapseAxis = use2DGrid; + if ( collapseAxis ) + { + // pick the smallest axis to collapse, leaving us with the greatest number of cells in our grid + int iAxisToCollapse = 0; + int axisDim = gridDim[iAxisToCollapse]; + //for each dimension + for ( int i = 0; i < 3; ++i ) + { + if (gridDim[i] < axisDim) + { + iAxisToCollapse = i; + axisDim = gridDim[i]; + } + } + // collapse it + gridCellSize[iAxisToCollapse] = gridExtent[iAxisToCollapse] * 2.0f; + phaseMask &= ~(1 << iAxisToCollapse); + } + + int numGridChunks = 0; + btIntVec3 gridChunkDim; // each chunk is 2x2x2 group of cells + while (true) + { + gridDim[0] = int( 1.0 + gridExtent.x() / gridCellSize.x() ); + gridDim[1] = int( 1.0 + gridExtent.y() / gridCellSize.y() ); + gridDim[2] = int( 1.0 + gridExtent.z() / gridCellSize.z() ); + gridChunkDim[ 0 ] = btMax( 1, ( gridDim[ 0 ] + 0 ) / 2 ); + gridChunkDim[ 1 ] = btMax( 1, ( gridDim[ 1 ] + 0 ) / 2 ); + gridChunkDim[ 2 ] = btMax( 1, ( gridDim[ 2 ] + 0 ) / 2 ); + numGridChunks = gridChunkDim[ 0 ] * gridChunkDim[ 1 ] * gridChunkDim[ 2 ]; + float nChunks = float(gridChunkDim[0]) * float(gridChunkDim[1]) * float(gridChunkDim[2]); // suceptible to integer overflow + if ( numGridChunks <= maxGridChunkCount && nChunks <= maxGridChunkCount ) + { + break; + } + gridCellSize *= 1.25; // should roughly cut numCells in half + } + btAssert(numGridChunks <= maxGridChunkCount ); + int maxNumBatchesPerPhase = numGridChunks; + + // for each dynamic body, compute grid coords + btVector3 invGridCellSize = btVector3(1,1,1)/gridCellSize; + // (can be done in parallel) + for (int iBody = 0; iBody < bodies.size(); ++iBody) + { + btIntVec3& coords = bodyGridCoords[iBody]; + if (bodyDynamicFlags[iBody]) + { + btVector3 v = ( bodyPositions[ iBody ] - bboxMin )*invGridCellSize; + coords.m_ints[0] = int(v.x()); + coords.m_ints[1] = int(v.y()); + coords.m_ints[2] = int(v.z()); + btAssert(coords.m_ints[0] >= 0 && coords.m_ints[0] < gridDim[0]); + btAssert(coords.m_ints[1] >= 0 && coords.m_ints[1] < gridDim[1]); + btAssert(coords.m_ints[2] >= 0 && coords.m_ints[2] < gridDim[2]); + } + else + { + coords.m_ints[0] = -1; + coords.m_ints[1] = -1; + coords.m_ints[2] = -1; + } + } + + for (int iPhase = 0; iPhase < numPhases; ++iPhase) + { + int batchBegin = iPhase * maxNumBatchesPerPhase; + int batchEnd = batchBegin + maxNumBatchesPerPhase; + for ( int iBatch = batchBegin; iBatch < batchEnd; ++iBatch ) + { + btBatchInfo& batch = batches[ iBatch ]; + batch = btBatchInfo(); + } + } + + { + AssignConstraintsToGridBatchesParams params; + params.bodyDynamicFlags = bodyDynamicFlags; + params.bodyGridCoords = bodyGridCoords; + params.numBodies = bodies.size(); + params.conInfos = conInfos; + params.constraintBatchIds = constraintBatchIds; + params.gridChunkDim = gridChunkDim; + params.maxNumBatchesPerPhase = maxNumBatchesPerPhase; + params.numPhases = numPhases; + params.phaseMask = phaseMask; + bool inParallel = true; + if (inParallel) + { + AssignConstraintsToGridBatchesLoop loop(params); + int grainSize = 250; + btParallelFor(0, numConstraints, grainSize, loop); + } + else + { + assignConstraintsToGridBatches( params, 0, numConstraints ); + } + } + for ( int iCon = 0; iCon < numConstraints; ++iCon ) + { + const btBatchedConstraintInfo& con = conInfos[ iCon ]; + int iBatch = constraintBatchIds[ iCon ]; + btBatchInfo& batch = batches[iBatch]; + batch.numConstraints += con.numConstraintRows; + } + + for (int iPhase = 0; iPhase < numPhases; ++iPhase) + { + // if phase is legit, + if (iPhase == (iPhase&phaseMask)) + { + int iBeginBatch = iPhase * maxNumBatchesPerPhase; + int iEndBatch = iBeginBatch + maxNumBatchesPerPhase; + mergeSmallBatches( batches, iBeginBatch, iEndBatch, minBatchSize, maxBatchSize ); + } + } + // all constraints have been assigned a batchId + updateConstraintBatchIdsForMergesMt(constraintBatchIds, numConstraints, batches, maxNumBatchesPerPhase*numPhases); + + if (numConstraintRows > numConstraints) + { + expandConstraintRowsMt(&constraintRowBatchIds[0], &constraintBatchIds[0], &conInfos[0], numConstraints, numConstraintRows); + } + else + { + constraintRowBatchIds = constraintBatchIds; + } + + writeOutBatches(batchedConstraints, constraintRowBatchIds, numConstraintRows, batches, batchWork, maxNumBatchesPerPhase, numPhases); + btAssert(batchedConstraints->validate(constraints, bodies)); +} + + +static void setupSingleBatch( + btBatchedConstraints* bc, + int numConstraints +) +{ + BT_PROFILE("setupSingleBatch"); + typedef btBatchedConstraints::Range Range; + + bc->m_constraintIndices.resize( numConstraints ); + for ( int i = 0; i < numConstraints; ++i ) + { + bc->m_constraintIndices[ i ] = i; + } + + bc->m_batches.resizeNoInitialize( 0 ); + bc->m_phases.resizeNoInitialize( 0 ); + bc->m_phaseOrder.resizeNoInitialize( 0 ); + bc->m_phaseGrainSize.resizeNoInitialize( 0 ); + + if (numConstraints > 0) + { + bc->m_batches.push_back( Range( 0, numConstraints ) ); + bc->m_phases.push_back( Range( 0, 1 ) ); + bc->m_phaseOrder.push_back(0); + bc->m_phaseGrainSize.push_back(1); + } +} + + +void btBatchedConstraints::setup( + btConstraintArray* constraints, + const btAlignedObjectArray& bodies, + BatchingMethod batchingMethod, + int minBatchSize, + int maxBatchSize, + btAlignedObjectArray* scratchMemory + ) +{ + if (constraints->size() >= minBatchSize*4) + { + bool use2DGrid = batchingMethod == BATCHING_METHOD_SPATIAL_GRID_2D; + setupSpatialGridBatchesMt( this, scratchMemory, constraints, bodies, minBatchSize, maxBatchSize, use2DGrid ); + if (s_debugDrawBatches) + { + debugDrawAllBatches( this, constraints, bodies ); + } + } + else + { + setupSingleBatch( this, constraints->size() ); + } +} + + diff --git a/extern/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h new file mode 100644 index 000000000000..0fd8f31dd453 --- /dev/null +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btBatchedConstraints.h @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_BATCHED_CONSTRAINTS_H +#define BT_BATCHED_CONSTRAINTS_H + +#include "LinearMath/btThreads.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "BulletDynamics/ConstraintSolver/btSolverBody.h" +#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h" + + +class btIDebugDraw; + +struct btBatchedConstraints +{ + enum BatchingMethod + { + BATCHING_METHOD_SPATIAL_GRID_2D, + BATCHING_METHOD_SPATIAL_GRID_3D, + BATCHING_METHOD_COUNT + }; + struct Range + { + int begin; + int end; + + Range() : begin( 0 ), end( 0 ) {} + Range( int _beg, int _end ) : begin( _beg ), end( _end ) {} + }; + + btAlignedObjectArray m_constraintIndices; + btAlignedObjectArray m_batches; // each batch is a range of indices in the m_constraintIndices array + btAlignedObjectArray m_phases; // each phase is range of indices in the m_batches array + btAlignedObjectArray m_phaseGrainSize; // max grain size for each phase + btAlignedObjectArray m_phaseOrder; // phases can be done in any order, so we can randomize the order here + btIDebugDraw* m_debugDrawer; + + static bool s_debugDrawBatches; + + btBatchedConstraints() {m_debugDrawer=NULL;} + void setup( btConstraintArray* constraints, + const btAlignedObjectArray& bodies, + BatchingMethod batchingMethod, + int minBatchSize, + int maxBatchSize, + btAlignedObjectArray* scratchMemory + ); + bool validate( btConstraintArray* constraints, const btAlignedObjectArray& bodies ) const; +}; + + +#endif // BT_BATCHED_CONSTRAINTS_H + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp similarity index 99% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 09b7388b63e0..0572256f74b0 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -642,7 +642,7 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr btTransform trDeltaAB = trB * trPose * trA.inverse(); btQuaternion qDeltaAB = trDeltaAB.getRotation(); btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); - float swingAxisLen2 = swingAxis.length2(); + btScalar swingAxisLen2 = swingAxis.length2(); if(btFuzzyZero(swingAxisLen2)) { return; @@ -903,7 +903,7 @@ btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btSc // a^2 b^2 // Do the math and it should be clear. - float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) + btScalar swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) if (fabs(xEllipse) > SIMD_EPSILON) { btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h similarity index 99% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index b7636180c345..7a33d01d1eb2 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -260,7 +260,7 @@ ATTRIBUTE_ALIGNED16(class) btConeTwistConstraint : public btTypedConstraint inline int getSolveSwingLimit() { - return m_solveTwistLimit; + return m_solveSwingLimit; } inline btScalar getTwistLimitSign() diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.h similarity index 99% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index 477c79d17565..adb2268353e0 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -28,11 +28,13 @@ ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint btPersistentManifold m_contactManifold; -public: +protected: btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB); +public: + void setContactManifold(btPersistentManifold* contactManifold); btPersistentManifold* getContactManifold() diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h similarity index 85% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index a3a0fa6729fe..93865cbc5920 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -29,7 +29,8 @@ enum btSolverMode SOLVER_CACHE_FRIENDLY = 128, SOLVER_SIMD = 256, SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512, - SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024 + SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024, + SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048 }; struct btContactSolverInfoData @@ -43,10 +44,13 @@ struct btContactSolverInfoData btScalar m_restitution; int m_numIterations; btScalar m_maxErrorReduction; - btScalar m_sor; - btScalar m_erp;//used as Baumgarte factor - btScalar m_erp2;//used in Split Impulse - btScalar m_globalCfm;//constraint force mixing + btScalar m_sor;//successive over-relaxation term + btScalar m_erp;//error reduction for non-contact constraints + btScalar m_erp2;//error reduction for contact constraints + btScalar m_globalCfm;//constraint force mixing for contacts and non-contacts + btScalar m_frictionERP;//error reduction for friction constraints + btScalar m_frictionCFM;//constraint force mixing for friction constraints + int m_splitImpulse; btScalar m_splitImpulsePenetrationThreshold; btScalar m_splitImpulseTurnErp; @@ -58,7 +62,8 @@ struct btContactSolverInfoData int m_minimumSolverBatchSize; btScalar m_maxGyroscopicForce; btScalar m_singleAxisRollingFrictionThreshold; - + btScalar m_leastSquaresResidualThreshold; + btScalar m_restitutionVelocityThreshold; }; @@ -77,8 +82,10 @@ struct btContactSolverInfo : public btContactSolverInfoData m_maxErrorReduction = btScalar(20.); m_numIterations = 10; m_erp = btScalar(0.2); - m_erp2 = btScalar(0.8); + m_erp2 = btScalar(0.2); m_globalCfm = btScalar(0.); + m_frictionERP = btScalar(0.2);//positional friction 'anchors' are disabled by default + m_frictionCFM = btScalar(0.); m_sor = btScalar(1.); m_splitImpulse = true; m_splitImpulsePenetrationThreshold = -.04f; @@ -91,6 +98,8 @@ struct btContactSolverInfo : public btContactSolverInfoData m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit m_maxGyroscopicForce = 100.f; ///it is only used for 'explicit' version of gyroscopic force m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows. + m_leastSquaresResidualThreshold = 0.f; + m_restitutionVelocityThreshold = 0.2f;//if the relative velocity is below this threshold, there is zero restitution } }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.h similarity index 95% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.h index f9afcb91211f..e4613455a2b8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGearConstraint.h @@ -141,6 +141,14 @@ SIMD_FORCE_INLINE const char* btGearConstraint::serialize(void* dataBuffer, btSe gear->m_ratio = m_ratio; + // Fill padding with zeros to appease msan. +#ifndef BT_USE_DOUBLE_PRECISION + gear->m_padding[0] = 0; + gear->m_padding[1] = 0; + gear->m_padding[2] = 0; + gear->m_padding[3] = 0; +#endif + return btGearConstraintDataName; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp similarity index 99% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index bc2b5a85df95..c38b8353f068 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -776,7 +776,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2( btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed) { int srow = row * info->rowskip; - int powered = limot->m_enableMotor; + bool 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 @@ -840,7 +840,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2( } // if we're limited low and high simultaneously, the joint motor is // ineffective - if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0; + if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false; info->m_constraintError[srow] = btScalar(0.f); if (powered) { @@ -855,8 +855,8 @@ int btGeneric6DofConstraint::get_limit_motor_info2( tag_vel, info->fps * limot->m_stopERP); info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity; - info->m_lowerLimit[srow] = -limot->m_maxMotorForce; - info->m_upperLimit[srow] = limot->m_maxMotorForce; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; } } if(limit) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h similarity index 99% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index bea8629c3259..b2ad45f749de 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -77,7 +77,7 @@ class btRotationalLimitMotor { m_accumulatedImpulse = 0.f; m_targetVelocity = 0; - m_maxMotorForce = 0.1f; + m_maxMotorForce = 6.0f; m_maxLimitForce = 300.0f; m_loLimit = 1.0f; m_hiLimit = -1.0f; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp similarity index 91% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp index 313719448645..611b3034264b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp @@ -1,1138 +1,1189 @@ -/* -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. -*/ - -/* -2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer -Pros: -- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled) -- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring) -- Servo motor functionality -- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision) -- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2) - -Cons: -- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. (with PGS) -- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.) -*/ - -/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev -/// Added support for generic constraint solver through getInfo1/getInfo2 methods - -/* -2007-09-09 -btGeneric6DofConstraint Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - - - -#include "btGeneric6DofSpring2Constraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include - - - -btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder) - : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB) - , m_frameInA(frameInA) - , m_frameInB(frameInB) - , m_rotateOrder(rotOrder) - , m_flags(0) -{ - calculateTransforms(); -} - - -btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder) - : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB) - , m_frameInB(frameInB) - , m_rotateOrder(rotOrder) - , m_flags(0) -{ - ///not providing rigidbody A means implicitly using worldspace for body A - m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; - calculateTransforms(); -} - - -btScalar btGeneric6DofSpring2Constraint::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 btGeneric6DofSpring2Constraint::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; -} - -bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz) -{ - // rot = cy*cz -sz sy*cz - // cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx - // cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy - - btScalar fi = btGetMatrixElem(mat,1); - if (fi < btScalar(1.0f)) - { - if (fi > btScalar(-1.0f)) - { - xyz[0] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,4)); - xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); - xyz[2] = btAsin(-btGetMatrixElem(mat,1)); - return true; - } - else - { - xyz[0] = -btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); - xyz[1] = btScalar(0.0); - xyz[2] = SIMD_HALF_PI; - return false; - } - } - else - { - xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); - xyz[1] = 0.0; - xyz[2] = -SIMD_HALF_PI; - } - return false; -} - -bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz) -{ - // rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy - // cx*sz cx*cz -sx - // cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx - - btScalar fi = btGetMatrixElem(mat,5); - if (fi < btScalar(1.0f)) - { - if (fi > btScalar(-1.0f)) - { - xyz[0] = btAsin(-btGetMatrixElem(mat,5)); - xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,8)); - xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); - return true; - } - else - { - xyz[0] = SIMD_HALF_PI; - xyz[1] = -btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); - xyz[2] = btScalar(0.0); - return false; - } - } - else - { - xyz[0] = -SIMD_HALF_PI; - xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); - xyz[2] = 0.0; - } - return false; -} - -bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz) -{ - // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx - // sz cz*cx -cz*sx - // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx - - btScalar fi = btGetMatrixElem(mat,3); - if (fi < btScalar(1.0f)) - { - if (fi > btScalar(-1.0f)) - { - xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,4)); - xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,0)); - xyz[2] = btAsin(btGetMatrixElem(mat,3)); - return true; - } - else - { - xyz[0] = btScalar(0.0); - xyz[1] = -btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8)); - xyz[2] = -SIMD_HALF_PI; - return false; - } - } - else - { - xyz[0] = btScalar(0.0); - xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8)); - xyz[2] = SIMD_HALF_PI; - } - return false; -} - -bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz) -{ - // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx - // cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx - // -cx*sy sx cx*cy - - btScalar fi = btGetMatrixElem(mat,7); - if (fi < btScalar(1.0f)) - { - if (fi > btScalar(-1.0f)) - { - xyz[0] = btAsin(btGetMatrixElem(mat,7)); - xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); - xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,4)); - return true; - } - else - { - xyz[0] = -SIMD_HALF_PI; - xyz[1] = btScalar(0.0); - xyz[2] = -btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); - return false; - } - } - else - { - xyz[0] = SIMD_HALF_PI; - xyz[1] = btScalar(0.0); - xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); - } - return false; -} - -bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz) -{ - // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy - // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx - // -sy cy*sx cy*cx - - btScalar fi = btGetMatrixElem(mat,6); - if (fi < btScalar(1.0f)) - { - if (fi > btScalar(-1.0f)) - { - xyz[0] = btAtan2(btGetMatrixElem(mat,7), btGetMatrixElem(mat,8)); - xyz[1] = btAsin(-btGetMatrixElem(mat,6)); - xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,0)); - return true; - } - else - { - xyz[0] = btScalar(0.0); - xyz[1] = SIMD_HALF_PI; - xyz[2] = -btAtan2(btGetMatrixElem(mat,1),btGetMatrixElem(mat,2)); - return false; - } - } - else - { - xyz[0] = btScalar(0.0); - xyz[1] = -SIMD_HALF_PI; - xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2)); - } - return false; -} - -void btGeneric6DofSpring2Constraint::calculateAngleInfo() -{ - btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); - switch (m_rotateOrder) - { - case RO_XYZ : matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); break; - case RO_XZY : matrixToEulerXZY(relative_frame,m_calculatedAxisAngleDiff); break; - case RO_YXZ : matrixToEulerYXZ(relative_frame,m_calculatedAxisAngleDiff); break; - case RO_YZX : matrixToEulerYZX(relative_frame,m_calculatedAxisAngleDiff); break; - case RO_ZXY : matrixToEulerZXY(relative_frame,m_calculatedAxisAngleDiff); break; - case RO_ZYX : matrixToEulerZYX(relative_frame,m_calculatedAxisAngleDiff); break; - default : btAssert(false); - } - // 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. - switch (m_rotateOrder) - { - case RO_XYZ : - { - //Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles) - //The two planes are non-homologous, so this is a Tait–Bryan angle formalism and not a proper Euler - //Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation) - //that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait–Bryan angles) - // x' = Nperp = N.cross(axis2) - // y' = N = axis2.cross(axis0) - // z' = z - // - // x" = X - // y" = y' - // z" = ?? - //in other words: - //first rotate around z - //second rotate around y'= z.cross(X) - //third rotate around x" = X - //Original XYZ extrinsic rotation order. - //Planes: xy and YZ normals: z, X. Plane intersection (N) is z.cross(X) - 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]); - break; - } - case RO_XZY : - { - //planes: xz,ZY normals: y, X - //first rotate around y - //second rotate around z'= y.cross(X) - //third rotate around x" = X - btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); - btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1); - m_calculatedAxis[2] = axis0.cross(axis1); - m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]); - m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0); - break; - } - case RO_YXZ : - { - //planes: yx,XZ normals: z, Y - //first rotate around z - //second rotate around x'= z.cross(Y) - //third rotate around y" = Y - btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1); - btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); - m_calculatedAxis[0] = axis1.cross(axis2); - m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]); - m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1); - break; - } - case RO_YZX : - { - //planes: yz,ZX normals: x, Y - //first rotate around x - //second rotate around z'= x.cross(Y) - //third rotate around y" = Y - btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0); - btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1); - m_calculatedAxis[2] = axis0.cross(axis1); - m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]); - m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0); - break; - } - case RO_ZXY : - { - //planes: zx,XY normals: y, Z - //first rotate around y - //second rotate around x'= y.cross(Z) - //third rotate around z" = Z - btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1); - btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2); - m_calculatedAxis[0] = axis1.cross(axis2); - m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]); - m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1); - break; - } - case RO_ZYX : - { - //planes: zy,YX normals: x, Z - //first rotate around x - //second rotate around y' = x.cross(Z) - //third rotate around z" = Z - btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0); - btVector3 axis2 = m_calculatedTransformB.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]); - break; - } - default: - btAssert(false); - } - - m_calculatedAxis[0].normalize(); - m_calculatedAxis[1].normalize(); - m_calculatedAxis[2].normalize(); - -} - -void btGeneric6DofSpring2Constraint::calculateTransforms() -{ - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); -} - -void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA,const btTransform& transB) -{ - m_calculatedTransformA = transA * m_frameInA; - m_calculatedTransformB = transB * m_frameInB; - calculateLinearInfo(); - calculateAngleInfo(); - - btScalar miA = getRigidBodyA().getInvMass(); - btScalar miB = getRigidBodyB().getInvMass(); - m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); - btScalar miS = miA + miB; - if(miS > btScalar(0.f)) - { - m_factA = miB / miS; - } - else - { - m_factA = btScalar(0.5f); - } - m_factB = btScalar(1.0f) - m_factA; -} - - -void btGeneric6DofSpring2Constraint::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; - m_angularLimits[axis_index].testLimitValue(angle); -} - - -void btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info) -{ - //prepare constraint - calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); - info->m_numConstraintRows = 0; - info->nub = 0; - int i; - //test linear limits - for(i = 0; i < 3; i++) - { - if (m_linearLimits.m_currentLimit[i]==4) info->m_numConstraintRows += 2; - else if (m_linearLimits.m_currentLimit[i]!=0) info->m_numConstraintRows += 1; - if (m_linearLimits.m_enableMotor[i] ) info->m_numConstraintRows += 1; - if (m_linearLimits.m_enableSpring[i]) info->m_numConstraintRows += 1; - } - //test angular limits - for (i=0;i<3 ;i++ ) - { - testAngularLimitMotor(i); - if (m_angularLimits[i].m_currentLimit==4) info->m_numConstraintRows += 2; - else if (m_angularLimits[i].m_currentLimit!=0) info->m_numConstraintRows += 1; - if (m_angularLimits[i].m_enableMotor ) info->m_numConstraintRows += 1; - if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1; - } -} - - -void btGeneric6DofSpring2Constraint::getInfo2 (btConstraintInfo2* info) -{ - const btTransform& transA = m_rbA.getCenterOfMassTransform(); - const btTransform& transB = m_rbB.getCenterOfMassTransform(); - const btVector3& linVelA = m_rbA.getLinearVelocity(); - const btVector3& linVelB = m_rbB.getLinearVelocity(); - const btVector3& angVelA = m_rbA.getAngularVelocity(); - const btVector3& angVelB = m_rbB.getAngularVelocity(); - - // for stability better to solve angular limits first - int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); - setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); -} - - -int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) -{ - //solve linear limits - btRotationalLimitMotor2 limot; - for (int i=0;i<3 ;i++ ) - { - if(m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i]) - { // re-use rotational motor code - limot.m_bounce = m_linearLimits.m_bounce[i]; - 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_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i]; - limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; - limot.m_servoMotor = m_linearLimits.m_servoMotor[i]; - limot.m_servoTarget = m_linearLimits.m_servoTarget[i]; - limot.m_enableSpring = m_linearLimits.m_enableSpring[i]; - limot.m_springStiffness = m_linearLimits.m_springStiffness[i]; - limot.m_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i]; - limot.m_springDamping = m_linearLimits.m_springDamping[i]; - limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[i]; - limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i]; - limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; - limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; - limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; - limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; - btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); - int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2); - limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; - limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp; - limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0]; - limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp; - - //rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible. - int indx1 = (i + 1) % 3; - int indx2 = (i + 2) % 3; - int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static) - #define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3 - bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 || - m_angularLimits[indx1].m_currentLimit == 2 || - ( m_angularLimits[indx1].m_currentLimit == 3 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) || - ( m_angularLimits[indx1].m_currentLimit == 4 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ); - bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 || - m_angularLimits[indx2].m_currentLimit == 2 || - ( m_angularLimits[indx2].m_currentLimit == 3 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) || - ( m_angularLimits[indx2].m_currentLimit == 4 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ); - if( indx1Violated && indx2Violated ) - { - rotAllowed = 0; - } - row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed); - - } - } - return row; -} - - - -int btGeneric6DofSpring2Constraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) -{ - int row = row_offset; - - //order of rotational constraint rows - int cIdx[] = {0, 1, 2}; - switch(m_rotateOrder) - { - case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break; - case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break; - case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break; - case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break; - case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break; - case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break; - default : btAssert(false); - } - - for (int ii = 0; ii < 3 ; ii++ ) - { - int i = cIdx[ii]; - if(m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring) - { - btVector3 axis = getAxis(i); - int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2); - if(!(flags & BT_6DOF_FLAGS_CFM_STOP2)) - { - m_angularLimits[i].m_stopCFM = info->cfm[0]; - } - if(!(flags & BT_6DOF_FLAGS_ERP_STOP2)) - { - m_angularLimits[i].m_stopERP = info->erp; - } - if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2)) - { - m_angularLimits[i].m_motorCFM = info->cfm[0]; - } - if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2)) - { - m_angularLimits[i].m_motorERP = info->erp; - } - row += get_limit_motor_info2(&m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1); - } - } - - return row; -} - - -void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB) -{ - m_frameInA = frameA; - m_frameInB = frameB; - buildJacobian(); - calculateTransforms(); -} - - -void btGeneric6DofSpring2Constraint::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]); - } -} - -void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2 *info, int srow, btVector3& ax1, int rotational, int rotAllowed) -{ - btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; - btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis; - - 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]; - - if(!rotational) - { - btVector3 tmpA, tmpB, relA, relB; - // get vector from bodyB to frameB in WCS - relB = m_calculatedTransformB.getOrigin() - transB.getOrigin(); - // same for bodyA - relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); - tmpA = relA.cross(ax1); - tmpB = relB.cross(ax1); - if(m_hasStaticBody && (!rotAllowed)) - { - tmpA *= m_factA; - tmpB *= m_factB; - } - int i; - for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i]; - } -} - - -int btGeneric6DofSpring2Constraint::get_limit_motor_info2( - btRotationalLimitMotor2 * 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 rotAllowed) -{ - int count = 0; - int srow = row * info->rowskip; - - if (limot->m_currentLimit==4) - { - btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1); - - calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); - info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1); - if (rotational) { - if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) { - btScalar bounceerror = -limot->m_bounce* vel; - if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; - } - } else { - if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) { - btScalar bounceerror = -limot->m_bounce* vel; - if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; - } - } - info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY; - info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0; - info->cfm[srow] = limot->m_stopCFM; - srow += info->rowskip; - ++count; - - calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); - info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1); - if (rotational) { - if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) { - btScalar bounceerror = -limot->m_bounce* vel; - if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; - } - } else { - if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) { - btScalar bounceerror = -limot->m_bounce* vel; - if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; - } - } - info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0; - info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY; - info->cfm[srow] = limot->m_stopCFM; - srow += info->rowskip; - ++count; - } else - if (limot->m_currentLimit==3) - { - calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); - info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1); - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - info->cfm[srow] = limot->m_stopCFM; - srow += info->rowskip; - ++count; - } - - if (limot->m_enableMotor && !limot->m_servoMotor) - { - calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); - 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 * limot->m_motorERP); - info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity; - info->m_lowerLimit[srow] = -limot->m_maxMotorForce; - info->m_upperLimit[srow] = limot->m_maxMotorForce; - info->cfm[srow] = limot->m_motorCFM; - srow += info->rowskip; - ++count; - } - - if (limot->m_enableMotor && limot->m_servoMotor) - { - btScalar error = limot->m_currentPosition - limot->m_servoTarget; - calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); - btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity; - btScalar tag_vel = -targetvelocity; - btScalar mot_fact; - if(error != 0) - { - btScalar lowLimit; - btScalar hiLimit; - if(limot->m_loLimit > limot->m_hiLimit) - { - lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY; - hiLimit = error < 0 ? limot->m_servoTarget : SIMD_INFINITY; - } - else - { - lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit; - hiLimit = error < 0 && limot->m_servoTargetm_hiLimit ? limot->m_servoTarget : limot->m_hiLimit; - } - mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP); - } - else - { - mot_fact = 0; - } - info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1); - info->m_lowerLimit[srow] = -limot->m_maxMotorForce; - info->m_upperLimit[srow] = limot->m_maxMotorForce; - info->cfm[srow] = limot->m_motorCFM; - srow += info->rowskip; - ++count; - } - - if (limot->m_enableSpring) - { - btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint; - calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); - - //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping); - //if(cfm > 0.99999) - // cfm = 0.99999; - //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping); - //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0); - //info->m_lowerLimit[srow] = -SIMD_INFINITY; - //info->m_upperLimit[srow] = SIMD_INFINITY; - - btScalar dt = BT_ONE / info->fps; - btScalar kd = limot->m_springDamping; - btScalar ks = limot->m_springStiffness; - btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1); -// btScalar erp = 0.1; - btScalar cfm = BT_ZERO; - btScalar mA = BT_ONE / m_rbA.getInvMass(); - btScalar mB = BT_ONE / m_rbB.getInvMass(); - if (rotational) { - btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2(); - btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2(); - if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length(); - if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length(); - } - btScalar m = mA > mB ? mB : mA; - btScalar angularfreq = sqrt(ks / m); - - - //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency) - if(limot->m_springStiffnessLimited && 0.25 < angularfreq * dt) - { - ks = BT_ONE / dt / dt / btScalar(16.0) * m; - } - //avoid damping that would blow up the spring - if(limot->m_springDampingLimited && kd * dt > m) - { - kd = m / dt; - } - btScalar fs = ks * error * dt; - btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt; - btScalar f = (fs+fd); - - // after the spring force affecting the body(es) the new velocity will be - // vel + f / m * (rotational ? -1 : 1) - // so in theory this should be set here for m_constraintError - // (with m_constraintError we set a desired velocity for the affected body(es)) - // however in practice any value is fine as long as it is greater then the "proper" velocity, - // because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force - // so it is much simpler (and more robust) just to simply use inf (with the proper sign) - // you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration) - // will we not request a velocity with the wrong direction ? - // and the answare is not, because in practice during the solving the current velocity is subtracted from the m_constraintError - // so the sign of the force that is really matters - info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY); - - btScalar minf = f < fd ? f : fd; - btScalar maxf = f < fd ? fd : f; - if(!rotational) - { - info->m_lowerLimit[srow] = minf > 0 ? 0 : minf; - info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf; - } - else - { - info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf; - info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf; - } - - info->cfm[srow] = cfm; - srow += info->rowskip; - ++count; - } - - return count; -} - - -//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). -//If no axis is provided, it uses the default axis for this constraint. -void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis) -{ - if((axis >= 0) && (axis < 3)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - m_linearLimits.m_stopERP[axis] = value; - m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); - break; - case BT_CONSTRAINT_STOP_CFM : - m_linearLimits.m_stopCFM[axis] = value; - m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); - break; - case BT_CONSTRAINT_ERP : - m_linearLimits.m_motorERP[axis] = value; - m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); - break; - case BT_CONSTRAINT_CFM : - m_linearLimits.m_motorCFM[axis] = value; - m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); - break; - default : - btAssertConstrParams(0); - } - } - else if((axis >=3) && (axis < 6)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - m_angularLimits[axis - 3].m_stopERP = value; - m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); - break; - case BT_CONSTRAINT_STOP_CFM : - m_angularLimits[axis - 3].m_stopCFM = value; - m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); - break; - case BT_CONSTRAINT_ERP : - m_angularLimits[axis - 3].m_motorERP = value; - m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); - break; - case BT_CONSTRAINT_CFM : - m_angularLimits[axis - 3].m_motorCFM = value; - m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); - break; - default : - btAssertConstrParams(0); - } - } - else - { - btAssertConstrParams(0); - } -} - -//return the local value of parameter -btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const -{ - btScalar retVal = 0; - if((axis >= 0) && (axis < 3)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); - retVal = m_linearLimits.m_stopERP[axis]; - break; - case BT_CONSTRAINT_STOP_CFM : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); - retVal = m_linearLimits.m_stopCFM[axis]; - break; - case BT_CONSTRAINT_ERP : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); - retVal = m_linearLimits.m_motorERP[axis]; - break; - case BT_CONSTRAINT_CFM : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); - retVal = m_linearLimits.m_motorCFM[axis]; - break; - default : - btAssertConstrParams(0); - } - } - else if((axis >=3) && (axis < 6)) - { - switch(num) - { - case BT_CONSTRAINT_STOP_ERP : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); - retVal = m_angularLimits[axis - 3].m_stopERP; - break; - case BT_CONSTRAINT_STOP_CFM : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); - retVal = m_angularLimits[axis - 3].m_stopCFM; - break; - case BT_CONSTRAINT_ERP : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); - retVal = m_angularLimits[axis - 3].m_motorERP; - break; - case BT_CONSTRAINT_CFM : - btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); - retVal = m_angularLimits[axis - 3].m_motorCFM; - break; - default : - btAssertConstrParams(0); - } - } - else - { - btAssertConstrParams(0); - } - return retVal; -} - - - -void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2) -{ - btVector3 zAxis = axis1.normalized(); - btVector3 yAxis = axis2.normalized(); - btVector3 xAxis = yAxis.cross(zAxis); // 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]); - - // now get constraint frame in local coordinate systems - m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; - m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; - - calculateTransforms(); -} - -void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce) -{ - btAssert((index >= 0) && (index < 6)); - if (index<3) - m_linearLimits.m_bounce[index] = bounce; - else - m_angularLimits[index - 3].m_bounce = bounce; -} - -void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff) -{ - btAssert((index >= 0) && (index < 6)); - if (index<3) - m_linearLimits.m_enableMotor[index] = onOff; - else - m_angularLimits[index - 3].m_enableMotor = onOff; -} - -void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff) -{ - btAssert((index >= 0) && (index < 6)); - if (index<3) - m_linearLimits.m_servoMotor[index] = onOff; - else - m_angularLimits[index - 3].m_servoMotor = onOff; -} - -void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity) -{ - btAssert((index >= 0) && (index < 6)); - if (index<3) - m_linearLimits.m_targetVelocity[index] = velocity; - else - m_angularLimits[index - 3].m_targetVelocity = velocity; -} - -void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target) -{ - btAssert((index >= 0) && (index < 6)); - if (index<3) - m_linearLimits.m_servoTarget[index] = target; - else - m_angularLimits[index - 3].m_servoTarget = target; -} - -void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force) -{ - btAssert((index >= 0) && (index < 6)); - if (index<3) - m_linearLimits.m_maxMotorForce[index] = force; - else - m_angularLimits[index - 3].m_maxMotorForce = force; -} - -void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff) -{ - btAssert((index >= 0) && (index < 6)); - if (index<3) - m_linearLimits.m_enableSpring[index] = onOff; - else - m_angularLimits[index - 3] .m_enableSpring = onOff; -} - -void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded) -{ - btAssert((index >= 0) && (index < 6)); - if (index<3) { - m_linearLimits.m_springStiffness[index] = stiffness; - m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded; - } else { - m_angularLimits[index - 3].m_springStiffness = stiffness; - m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded; - } -} - -void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded) -{ - btAssert((index >= 0) && (index < 6)); - if (index<3) { - m_linearLimits.m_springDamping[index] = damping; - m_linearLimits.m_springDampingLimited[index] = limitIfNeeded; - } else { - m_angularLimits[index - 3].m_springDamping = damping; - m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded; - } -} - -void btGeneric6DofSpring2Constraint::setEquilibriumPoint() -{ - calculateTransforms(); - int i; - for( i = 0; i < 3; i++) - m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; - for(i = 0; i < 3; i++) - m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i]; -} - -void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index) -{ - btAssert((index >= 0) && (index < 6)); - calculateTransforms(); - if (index<3) - m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; - else - m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3]; -} - -void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val) -{ - btAssert((index >= 0) && (index < 6)); - if (index<3) - m_linearLimits.m_equilibriumPoint[index] = val; - else - m_angularLimits[index - 3] .m_equilibriumPoint = val; -} - - -//////////////////////////// btRotationalLimitMotor2 //////////////////////////////////// - -void btRotationalLimitMotor2::testLimitValue(btScalar test_value) -{ - //we can't normalize the angles here because we would lost the sign that we use later, but it doesn't seem to be a problem - if(m_loLimit > m_hiLimit) { - m_currentLimit = 0; - m_currentLimitError = btScalar(0.f); - } - else if(m_loLimit == m_hiLimit) { - m_currentLimitError = test_value - m_loLimit; - m_currentLimit = 3; - } else { - m_currentLimitError = test_value - m_loLimit; - m_currentLimitErrorHi = test_value - m_hiLimit; - m_currentLimit = 4; - } -} - -//////////////////////////// btTranslationalLimitMotor2 //////////////////////////////////// - -void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_value) -{ - btScalar loLimit = m_lowerLimit[limitIndex]; - btScalar hiLimit = m_upperLimit[limitIndex]; - if(loLimit > hiLimit) { - m_currentLimitError[limitIndex] = 0; - m_currentLimit[limitIndex] = 0; - } - else if(loLimit == hiLimit) { - m_currentLimitError[limitIndex] = test_value - loLimit; - m_currentLimit[limitIndex] = 3; - } else { - m_currentLimitError[limitIndex] = test_value - loLimit; - m_currentLimitErrorHi[limitIndex] = test_value - hiLimit; - m_currentLimit[limitIndex] = 4; - } -} - - +/* +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. +*/ + +/* +2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer +Pros: +- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled) +- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring) +- Servo motor functionality +- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision) +- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2) + +Cons: +- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. (with PGS) +- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.) +*/ + +/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev +/// Added support for generic constraint solver through getInfo1/getInfo2 methods + +/* +2007-09-09 +btGeneric6DofConstraint Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + + + +#include "btGeneric6DofSpring2Constraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" +#include + + + +btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder) + : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB) + , m_frameInA(frameInA) + , m_frameInB(frameInB) + , m_rotateOrder(rotOrder) + , m_flags(0) +{ + calculateTransforms(); +} + + +btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder) + : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB) + , m_frameInB(frameInB) + , m_rotateOrder(rotOrder) + , m_flags(0) +{ + ///not providing rigidbody A means implicitly using worldspace for body A + m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; + calculateTransforms(); +} + + +btScalar btGeneric6DofSpring2Constraint::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 btGeneric6DofSpring2Constraint::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; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz -sz sy*cz + // cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx + // cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy + + btScalar fi = btGetMatrixElem(mat,1); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,4)); + xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); + xyz[2] = btAsin(-btGetMatrixElem(mat,1)); + return true; + } + else + { + xyz[0] = -btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); + xyz[1] = btScalar(0.0); + xyz[2] = SIMD_HALF_PI; + return false; + } + } + else + { + xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); + xyz[1] = 0.0; + xyz[2] = -SIMD_HALF_PI; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy + // cx*sz cx*cz -sx + // cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx + + btScalar fi = btGetMatrixElem(mat,5); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAsin(-btGetMatrixElem(mat,5)); + xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,8)); + xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4)); + return true; + } + else + { + xyz[0] = SIMD_HALF_PI; + xyz[1] = -btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + xyz[2] = btScalar(0.0); + return false; + } + } + else + { + xyz[0] = -SIMD_HALF_PI; + xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0)); + xyz[2] = 0.0; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx + // sz cz*cx -cz*sx + // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx + + btScalar fi = btGetMatrixElem(mat,3); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,4)); + xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,0)); + xyz[2] = btAsin(btGetMatrixElem(mat,3)); + return true; + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = -btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8)); + xyz[2] = -SIMD_HALF_PI; + return false; + } + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8)); + xyz[2] = SIMD_HALF_PI; + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx + // cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx + // -cx*sy sx cx*cy + + btScalar fi = btGetMatrixElem(mat,7); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAsin(btGetMatrixElem(mat,7)); + xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8)); + xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,4)); + return true; + } + else + { + xyz[0] = -SIMD_HALF_PI; + xyz[1] = btScalar(0.0); + xyz[2] = -btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); + return false; + } + } + else + { + xyz[0] = SIMD_HALF_PI; + xyz[1] = btScalar(0.0); + xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0)); + } + return false; +} + +bool btGeneric6DofSpring2Constraint::matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz) +{ + // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy + // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx + // -sy cy*sx cy*cx + + btScalar fi = btGetMatrixElem(mat,6); + if (fi < btScalar(1.0f)) + { + if (fi > btScalar(-1.0f)) + { + xyz[0] = btAtan2(btGetMatrixElem(mat,7), btGetMatrixElem(mat,8)); + xyz[1] = btAsin(-btGetMatrixElem(mat,6)); + xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,0)); + return true; + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = SIMD_HALF_PI; + xyz[2] = -btAtan2(btGetMatrixElem(mat,1),btGetMatrixElem(mat,2)); + return false; + } + } + else + { + xyz[0] = btScalar(0.0); + xyz[1] = -SIMD_HALF_PI; + xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2)); + } + return false; +} + +void btGeneric6DofSpring2Constraint::calculateAngleInfo() +{ + btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis(); + switch (m_rotateOrder) + { + case RO_XYZ : matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_XZY : matrixToEulerXZY(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_YXZ : matrixToEulerYXZ(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_YZX : matrixToEulerYZX(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_ZXY : matrixToEulerZXY(relative_frame,m_calculatedAxisAngleDiff); break; + case RO_ZYX : matrixToEulerZYX(relative_frame,m_calculatedAxisAngleDiff); break; + default : btAssert(false); + } + // 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. + switch (m_rotateOrder) + { + case RO_XYZ : + { + //Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles) + //The two planes are non-homologous, so this is a Tait–Bryan angle formalism and not a proper Euler + //Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation) + //that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait–Bryan angles) + // x' = Nperp = N.cross(axis2) + // y' = N = axis2.cross(axis0) + // z' = z + // + // x" = X + // y" = y' + // z" = ?? + //in other words: + //first rotate around z + //second rotate around y'= z.cross(X) + //third rotate around x" = X + //Original XYZ extrinsic rotation order. + //Planes: xy and YZ normals: z, X. Plane intersection (N) is z.cross(X) + 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]); + break; + } + case RO_XZY : + { + //planes: xz,ZY normals: y, X + //first rotate around y + //second rotate around z'= y.cross(X) + //third rotate around x" = X + btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0); + btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1); + m_calculatedAxis[2] = axis0.cross(axis1); + m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]); + m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0); + break; + } + case RO_YXZ : + { + //planes: yx,XZ normals: z, Y + //first rotate around z + //second rotate around x'= z.cross(Y) + //third rotate around y" = Y + btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1); + btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2); + m_calculatedAxis[0] = axis1.cross(axis2); + m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]); + m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1); + break; + } + case RO_YZX : + { + //planes: yz,ZX normals: x, Y + //first rotate around x + //second rotate around z'= x.cross(Y) + //third rotate around y" = Y + btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0); + btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1); + m_calculatedAxis[2] = axis0.cross(axis1); + m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]); + m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0); + break; + } + case RO_ZXY : + { + //planes: zx,XY normals: y, Z + //first rotate around y + //second rotate around x'= y.cross(Z) + //third rotate around z" = Z + btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1); + btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2); + m_calculatedAxis[0] = axis1.cross(axis2); + m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]); + m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1); + break; + } + case RO_ZYX : + { + //planes: zy,YX normals: x, Z + //first rotate around x + //second rotate around y' = x.cross(Z) + //third rotate around z" = Z + btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0); + btVector3 axis2 = m_calculatedTransformB.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]); + break; + } + default: + btAssert(false); + } + + m_calculatedAxis[0].normalize(); + m_calculatedAxis[1].normalize(); + m_calculatedAxis[2].normalize(); + +} + +void btGeneric6DofSpring2Constraint::calculateTransforms() +{ + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); +} + +void btGeneric6DofSpring2Constraint::calculateTransforms(const btTransform& transA,const btTransform& transB) +{ + m_calculatedTransformA = transA * m_frameInA; + m_calculatedTransformB = transB * m_frameInB; + calculateLinearInfo(); + calculateAngleInfo(); + + btScalar miA = getRigidBodyA().getInvMass(); + btScalar miB = getRigidBodyB().getInvMass(); + m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); + btScalar miS = miA + miB; + if(miS > btScalar(0.f)) + { + m_factA = miB / miS; + } + else + { + m_factA = btScalar(0.5f); + } + m_factB = btScalar(1.0f) - m_factA; +} + + +void btGeneric6DofSpring2Constraint::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; + m_angularLimits[axis_index].testLimitValue(angle); +} + + +void btGeneric6DofSpring2Constraint::getInfo1 (btConstraintInfo1* info) +{ + //prepare constraint + calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); + info->m_numConstraintRows = 0; + info->nub = 0; + int i; + //test linear limits + for(i = 0; i < 3; i++) + { + if (m_linearLimits.m_currentLimit[i]==4) info->m_numConstraintRows += 2; + else if (m_linearLimits.m_currentLimit[i]!=0) info->m_numConstraintRows += 1; + if (m_linearLimits.m_enableMotor[i] ) info->m_numConstraintRows += 1; + if (m_linearLimits.m_enableSpring[i]) info->m_numConstraintRows += 1; + } + //test angular limits + for (i=0;i<3 ;i++ ) + { + testAngularLimitMotor(i); + if (m_angularLimits[i].m_currentLimit==4) info->m_numConstraintRows += 2; + else if (m_angularLimits[i].m_currentLimit!=0) info->m_numConstraintRows += 1; + if (m_angularLimits[i].m_enableMotor ) info->m_numConstraintRows += 1; + if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1; + } +} + + +void btGeneric6DofSpring2Constraint::getInfo2 (btConstraintInfo2* info) +{ + const btTransform& transA = m_rbA.getCenterOfMassTransform(); + const btTransform& transB = m_rbB.getCenterOfMassTransform(); + const btVector3& linVelA = m_rbA.getLinearVelocity(); + const btVector3& linVelB = m_rbB.getLinearVelocity(); + const btVector3& angVelA = m_rbA.getAngularVelocity(); + const btVector3& angVelB = m_rbB.getAngularVelocity(); + + // for stability better to solve angular limits first + int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB); + setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB); +} + + +int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) +{ + //solve linear limits + btRotationalLimitMotor2 limot; + for (int i=0;i<3 ;i++ ) + { + if(m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i]) + { // re-use rotational motor code + limot.m_bounce = m_linearLimits.m_bounce[i]; + 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_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i]; + limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; + limot.m_servoMotor = m_linearLimits.m_servoMotor[i]; + limot.m_servoTarget = m_linearLimits.m_servoTarget[i]; + limot.m_enableSpring = m_linearLimits.m_enableSpring[i]; + limot.m_springStiffness = m_linearLimits.m_springStiffness[i]; + limot.m_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i]; + limot.m_springDamping = m_linearLimits.m_springDamping[i]; + limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[i]; + limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i]; + limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; + limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; + limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; + limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; + btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i); + int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2); + limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0]; + limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp; + limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0]; + limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp; + + //rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible. + int indx1 = (i + 1) % 3; + int indx2 = (i + 2) % 3; + int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static) + #define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3 + bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 || + m_angularLimits[indx1].m_currentLimit == 2 || + ( m_angularLimits[indx1].m_currentLimit == 3 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) || + ( m_angularLimits[indx1].m_currentLimit == 4 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ); + bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 || + m_angularLimits[indx2].m_currentLimit == 2 || + ( m_angularLimits[indx2].m_currentLimit == 3 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) || + ( m_angularLimits[indx2].m_currentLimit == 4 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ); + if( indx1Violated && indx2Violated ) + { + rotAllowed = 0; + } + row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed); + + } + } + return row; +} + + + +int btGeneric6DofSpring2Constraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB) +{ + int row = row_offset; + + //order of rotational constraint rows + int cIdx[] = {0, 1, 2}; + switch(m_rotateOrder) + { + case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break; + case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break; + case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break; + case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break; + case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break; + case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break; + default : btAssert(false); + } + + for (int ii = 0; ii < 3 ; ii++ ) + { + int i = cIdx[ii]; + if(m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring) + { + btVector3 axis = getAxis(i); + int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2); + if(!(flags & BT_6DOF_FLAGS_CFM_STOP2)) + { + m_angularLimits[i].m_stopCFM = info->cfm[0]; + } + if(!(flags & BT_6DOF_FLAGS_ERP_STOP2)) + { + m_angularLimits[i].m_stopERP = info->erp; + } + if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2)) + { + m_angularLimits[i].m_motorCFM = info->cfm[0]; + } + if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2)) + { + m_angularLimits[i].m_motorERP = info->erp; + } + row += get_limit_motor_info2(&m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1); + } + } + + return row; +} + + +void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB) +{ + m_frameInA = frameA; + m_frameInB = frameB; + buildJacobian(); + calculateTransforms(); +} + + +void btGeneric6DofSpring2Constraint::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]); + } +} + +void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2 *info, int srow, btVector3& ax1, int rotational, int rotAllowed) +{ + btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; + btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis; + + 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]; + + if(!rotational) + { + btVector3 tmpA, tmpB, relA, relB; + // get vector from bodyB to frameB in WCS + relB = m_calculatedTransformB.getOrigin() - transB.getOrigin(); + // same for bodyA + relA = m_calculatedTransformA.getOrigin() - transA.getOrigin(); + tmpA = relA.cross(ax1); + tmpB = relB.cross(ax1); + if(m_hasStaticBody && (!rotAllowed)) + { + tmpA *= m_factA; + tmpB *= m_factB; + } + int i; + for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i]; + } +} + + +int btGeneric6DofSpring2Constraint::get_limit_motor_info2( + btRotationalLimitMotor2 * 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 rotAllowed) +{ + int count = 0; + int srow = row * info->rowskip; + + if (limot->m_currentLimit==4) + { + btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1); + + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1); + if (rotational) { + if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } else { + if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } + info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY; + info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0; + info->cfm[srow] = limot->m_stopCFM; + srow += info->rowskip; + ++count; + + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1); + if (rotational) { + if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } else { + if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) { + btScalar bounceerror = -limot->m_bounce* vel; + if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror; + } + } + info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0; + info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY; + info->cfm[srow] = limot->m_stopCFM; + srow += info->rowskip; + ++count; + } else + if (limot->m_currentLimit==3) + { + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1); + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + info->cfm[srow] = limot->m_stopCFM; + srow += info->rowskip; + ++count; + } + + if (limot->m_enableMotor && !limot->m_servoMotor) + { + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + 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 * limot->m_motorERP); + info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity; + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; + info->cfm[srow] = limot->m_motorCFM; + srow += info->rowskip; + ++count; + } + + if (limot->m_enableMotor && limot->m_servoMotor) + { + btScalar error = limot->m_currentPosition - limot->m_servoTarget; + btScalar curServoTarget = limot->m_servoTarget; + if (rotational) + { + if (error > SIMD_PI) + { + error -= SIMD_2_PI; + curServoTarget +=SIMD_2_PI; + } + if (error < -SIMD_PI) + { + error += SIMD_2_PI; + curServoTarget -=SIMD_2_PI; + } + } + + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity; + btScalar tag_vel = -targetvelocity; + btScalar mot_fact; + if(error != 0) + { + btScalar lowLimit; + btScalar hiLimit; + if(limot->m_loLimit > limot->m_hiLimit) + { + lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY; + hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY; + } + else + { + lowLimit = error > 0 && curServoTarget>limot->m_loLimit ? curServoTarget : limot->m_loLimit; + hiLimit = error < 0 && curServoTargetm_hiLimit ? curServoTarget : limot->m_hiLimit; + } + mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP); + } + else + { + mot_fact = 0; + } + info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1); + info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps; + info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps; + info->cfm[srow] = limot->m_motorCFM; + srow += info->rowskip; + ++count; + } + + if (limot->m_enableSpring) + { + btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint; + calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); + + //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping); + //if(cfm > 0.99999) + // cfm = 0.99999; + //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping); + //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0); + //info->m_lowerLimit[srow] = -SIMD_INFINITY; + //info->m_upperLimit[srow] = SIMD_INFINITY; + + btScalar dt = BT_ONE / info->fps; + btScalar kd = limot->m_springDamping; + btScalar ks = limot->m_springStiffness; + btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1); +// btScalar erp = 0.1; + btScalar cfm = BT_ZERO; + btScalar mA = BT_ONE / m_rbA.getInvMass(); + btScalar mB = BT_ONE / m_rbB.getInvMass(); + if (rotational) { + btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2(); + btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2(); + if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length(); + if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length(); + } + btScalar m = mA > mB ? mB : mA; + btScalar angularfreq = sqrt(ks / m); + + + //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency) + if(limot->m_springStiffnessLimited && 0.25 < angularfreq * dt) + { + ks = BT_ONE / dt / dt / btScalar(16.0) * m; + } + //avoid damping that would blow up the spring + if(limot->m_springDampingLimited && kd * dt > m) + { + kd = m / dt; + } + btScalar fs = ks * error * dt; + btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt; + btScalar f = (fs+fd); + + // after the spring force affecting the body(es) the new velocity will be + // vel + f / m * (rotational ? -1 : 1) + // so in theory this should be set here for m_constraintError + // (with m_constraintError we set a desired velocity for the affected body(es)) + // however in practice any value is fine as long as it is greater then the "proper" velocity, + // because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force + // so it is much simpler (and more robust) just to simply use inf (with the proper sign) + // you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration) + // will we not request a velocity with the wrong direction ? + // and the answare is not, because in practice during the solving the current velocity is subtracted from the m_constraintError + // so the sign of the force that is really matters + info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY); + + btScalar minf = f < fd ? f : fd; + btScalar maxf = f < fd ? fd : f; + if(!rotational) + { + info->m_lowerLimit[srow] = minf > 0 ? 0 : minf; + info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf; + } + else + { + info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf; + info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf; + } + + info->cfm[srow] = cfm; + srow += info->rowskip; + ++count; + } + + return count; +} + + +//override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). +//If no axis is provided, it uses the default axis for this constraint. +void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis) +{ + if((axis >= 0) && (axis < 3)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + m_linearLimits.m_stopERP[axis] = value; + m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_STOP_CFM : + m_linearLimits.m_stopCFM[axis] = value; + m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_ERP : + m_linearLimits.m_motorERP[axis] = value; + m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_CFM : + m_linearLimits.m_motorCFM[axis] = value; + m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + default : + btAssertConstrParams(0); + } + } + else if((axis >=3) && (axis < 6)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + m_angularLimits[axis - 3].m_stopERP = value; + m_flags |= BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_STOP_CFM : + m_angularLimits[axis - 3].m_stopCFM = value; + m_flags |= BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_ERP : + m_angularLimits[axis - 3].m_motorERP = value; + m_flags |= BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + case BT_CONSTRAINT_CFM : + m_angularLimits[axis - 3].m_motorCFM = value; + m_flags |= BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2); + break; + default : + btAssertConstrParams(0); + } + } + else + { + btAssertConstrParams(0); + } +} + +//return the local value of parameter +btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const +{ + btScalar retVal = 0; + if((axis >= 0) && (axis < 3)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_stopERP[axis]; + break; + case BT_CONSTRAINT_STOP_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_stopCFM[axis]; + break; + case BT_CONSTRAINT_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_motorERP[axis]; + break; + case BT_CONSTRAINT_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_linearLimits.m_motorCFM[axis]; + break; + default : + btAssertConstrParams(0); + } + } + else if((axis >=3) && (axis < 6)) + { + switch(num) + { + case BT_CONSTRAINT_STOP_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_stopERP; + break; + case BT_CONSTRAINT_STOP_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_stopCFM; + break; + case BT_CONSTRAINT_ERP : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_motorERP; + break; + case BT_CONSTRAINT_CFM : + btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2))); + retVal = m_angularLimits[axis - 3].m_motorCFM; + break; + default : + btAssertConstrParams(0); + } + } + else + { + btAssertConstrParams(0); + } + return retVal; +} + + + +void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1,const btVector3& axis2) +{ + btVector3 zAxis = axis1.normalized(); + btVector3 yAxis = axis2.normalized(); + btVector3 xAxis = yAxis.cross(zAxis); // 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]); + + // now get constraint frame in local coordinate systems + m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW; + m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; + + calculateTransforms(); +} + +void btGeneric6DofSpring2Constraint::setBounce(int index, btScalar bounce) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_bounce[index] = bounce; + else + m_angularLimits[index - 3].m_bounce = bounce; +} + +void btGeneric6DofSpring2Constraint::enableMotor(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_enableMotor[index] = onOff; + else + m_angularLimits[index - 3].m_enableMotor = onOff; +} + +void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_servoMotor[index] = onOff; + else + m_angularLimits[index - 3].m_servoMotor = onOff; +} + +void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar velocity) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_targetVelocity[index] = velocity; + else + m_angularLimits[index - 3].m_targetVelocity = velocity; +} + + + +void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOrg) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + { + m_linearLimits.m_servoTarget[index] = targetOrg; + } + else + { + //wrap between -PI and PI, see also + //https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi + + btScalar target = targetOrg+SIMD_PI; + if (1) + { + btScalar m = target - SIMD_2_PI * floor(target/SIMD_2_PI); + // handle boundary cases resulted from floating-point cut off: + { + if (m>=SIMD_2_PI) + { + target = 0; + } else + { + if (m<0 ) + { + if (SIMD_2_PI+m == SIMD_2_PI) + target = 0; + else + target = SIMD_2_PI+m; + } + else + { + target = m; + } + } + } + target -= SIMD_PI; + } + + m_angularLimits[index - 3].m_servoTarget = target; + } +} + +void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_maxMotorForce[index] = force; + else + m_angularLimits[index - 3].m_maxMotorForce = force; +} + +void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_enableSpring[index] = onOff; + else + m_angularLimits[index - 3] .m_enableSpring = onOff; +} + +void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) { + m_linearLimits.m_springStiffness[index] = stiffness; + m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded; + } else { + m_angularLimits[index - 3].m_springStiffness = stiffness; + m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded; + } +} + +void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) { + m_linearLimits.m_springDamping[index] = damping; + m_linearLimits.m_springDampingLimited[index] = limitIfNeeded; + } else { + m_angularLimits[index - 3].m_springDamping = damping; + m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded; + } +} + +void btGeneric6DofSpring2Constraint::setEquilibriumPoint() +{ + calculateTransforms(); + int i; + for( i = 0; i < 3; i++) + m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i]; + for(i = 0; i < 3; i++) + m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i]; +} + +void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index) +{ + btAssert((index >= 0) && (index < 6)); + calculateTransforms(); + if (index<3) + m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index]; + else + m_angularLimits[index - 3] .m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3]; +} + +void btGeneric6DofSpring2Constraint::setEquilibriumPoint(int index, btScalar val) +{ + btAssert((index >= 0) && (index < 6)); + if (index<3) + m_linearLimits.m_equilibriumPoint[index] = val; + else + m_angularLimits[index - 3] .m_equilibriumPoint = val; +} + + +//////////////////////////// btRotationalLimitMotor2 //////////////////////////////////// + +void btRotationalLimitMotor2::testLimitValue(btScalar test_value) +{ + //we can't normalize the angles here because we would lost the sign that we use later, but it doesn't seem to be a problem + if(m_loLimit > m_hiLimit) { + m_currentLimit = 0; + m_currentLimitError = btScalar(0.f); + } + else if(m_loLimit == m_hiLimit) { + m_currentLimitError = test_value - m_loLimit; + m_currentLimit = 3; + } else { + m_currentLimitError = test_value - m_loLimit; + m_currentLimitErrorHi = test_value - m_hiLimit; + m_currentLimit = 4; + } +} + +//////////////////////////// btTranslationalLimitMotor2 //////////////////////////////////// + +void btTranslationalLimitMotor2::testLimitValue(int limitIndex, btScalar test_value) +{ + btScalar loLimit = m_lowerLimit[limitIndex]; + btScalar hiLimit = m_upperLimit[limitIndex]; + if(loLimit > hiLimit) { + m_currentLimitError[limitIndex] = 0; + m_currentLimit[limitIndex] = 0; + } + else if(loLimit == hiLimit) { + m_currentLimitError[limitIndex] = test_value - loLimit; + m_currentLimit[limitIndex] = 3; + } else { + m_currentLimitError[limitIndex] = test_value - loLimit; + m_currentLimitErrorHi[limitIndex] = test_value - hiLimit; + m_currentLimit[limitIndex] = 4; + } +} + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h similarity index 95% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h index ace4b3c29bf2..fc2ed4f7a989 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h @@ -1,674 +1,679 @@ -/* -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. -*/ - -/* -2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer -Pros: -- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled) -- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring) -- Servo motor functionality -- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision) -- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2) - -Cons: -- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. -- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.) -*/ - -/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev -/// Added support for generic constraint solver through getInfo1/getInfo2 methods - -/* -2007-09-09 -btGeneric6DofConstraint Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - - -#ifndef BT_GENERIC_6DOF_CONSTRAINT2_H -#define BT_GENERIC_6DOF_CONSTRAINT2_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -class btRigidBody; - - -#ifdef BT_USE_DOUBLE_PRECISION -#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2 -#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2" -#else -#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData -#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData" -#endif //BT_USE_DOUBLE_PRECISION - -enum RotateOrder -{ - RO_XYZ=0, - RO_XZY, - RO_YXZ, - RO_YZX, - RO_ZXY, - RO_ZYX -}; - -class btRotationalLimitMotor2 -{ -public: -// upper < lower means free -// upper == lower means locked -// upper > lower means limited - btScalar m_loLimit; - btScalar m_hiLimit; - btScalar m_bounce; - btScalar m_stopERP; - btScalar m_stopCFM; - btScalar m_motorERP; - btScalar m_motorCFM; - bool m_enableMotor; - btScalar m_targetVelocity; - btScalar m_maxMotorForce; - bool m_servoMotor; - btScalar m_servoTarget; - bool m_enableSpring; - btScalar m_springStiffness; - bool m_springStiffnessLimited; - btScalar m_springDamping; - bool m_springDampingLimited; - btScalar m_equilibriumPoint; - - btScalar m_currentLimitError; - btScalar m_currentLimitErrorHi; - btScalar m_currentPosition; - int m_currentLimit; - - btRotationalLimitMotor2() - { - m_loLimit = 1.0f; - m_hiLimit = -1.0f; - m_bounce = 0.0f; - m_stopERP = 0.2f; - m_stopCFM = 0.f; - m_motorERP = 0.9f; - m_motorCFM = 0.f; - m_enableMotor = false; - m_targetVelocity = 0; - m_maxMotorForce = 0.1f; - m_servoMotor = false; - m_servoTarget = 0; - m_enableSpring = false; - m_springStiffness = 0; - m_springStiffnessLimited = false; - m_springDamping = 0; - m_springDampingLimited = false; - m_equilibriumPoint = 0; - - m_currentLimitError = 0; - m_currentLimitErrorHi = 0; - m_currentPosition = 0; - m_currentLimit = 0; - } - - btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot) - { - m_loLimit = limot.m_loLimit; - m_hiLimit = limot.m_hiLimit; - m_bounce = limot.m_bounce; - m_stopERP = limot.m_stopERP; - m_stopCFM = limot.m_stopCFM; - m_motorERP = limot.m_motorERP; - m_motorCFM = limot.m_motorCFM; - m_enableMotor = limot.m_enableMotor; - m_targetVelocity = limot.m_targetVelocity; - m_maxMotorForce = limot.m_maxMotorForce; - m_servoMotor = limot.m_servoMotor; - m_servoTarget = limot.m_servoTarget; - m_enableSpring = limot.m_enableSpring; - m_springStiffness = limot.m_springStiffness; - m_springStiffnessLimited = limot.m_springStiffnessLimited; - m_springDamping = limot.m_springDamping; - m_springDampingLimited = limot.m_springDampingLimited; - m_equilibriumPoint = limot.m_equilibriumPoint; - - m_currentLimitError = limot.m_currentLimitError; - m_currentLimitErrorHi = limot.m_currentLimitErrorHi; - m_currentPosition = limot.m_currentPosition; - m_currentLimit = limot.m_currentLimit; - } - - - bool isLimited() - { - if(m_loLimit > m_hiLimit) return false; - return true; - } - - void testLimitValue(btScalar test_value); -}; - - - -class btTranslationalLimitMotor2 -{ -public: -// upper < lower means free -// upper == lower means locked -// upper > lower means limited - btVector3 m_lowerLimit; - btVector3 m_upperLimit; - btVector3 m_bounce; - btVector3 m_stopERP; - btVector3 m_stopCFM; - btVector3 m_motorERP; - btVector3 m_motorCFM; - bool m_enableMotor[3]; - bool m_servoMotor[3]; - bool m_enableSpring[3]; - btVector3 m_servoTarget; - btVector3 m_springStiffness; - bool m_springStiffnessLimited[3]; - btVector3 m_springDamping; - bool m_springDampingLimited[3]; - btVector3 m_equilibriumPoint; - btVector3 m_targetVelocity; - btVector3 m_maxMotorForce; - - btVector3 m_currentLimitError; - btVector3 m_currentLimitErrorHi; - btVector3 m_currentLinearDiff; - int m_currentLimit[3]; - - btTranslationalLimitMotor2() - { - m_lowerLimit .setValue(0.f , 0.f , 0.f ); - m_upperLimit .setValue(0.f , 0.f , 0.f ); - m_bounce .setValue(0.f , 0.f , 0.f ); - m_stopERP .setValue(0.2f, 0.2f, 0.2f); - m_stopCFM .setValue(0.f , 0.f , 0.f ); - m_motorERP .setValue(0.9f, 0.9f, 0.9f); - m_motorCFM .setValue(0.f , 0.f , 0.f ); - - m_currentLimitError .setValue(0.f , 0.f , 0.f ); - m_currentLimitErrorHi.setValue(0.f , 0.f , 0.f ); - m_currentLinearDiff .setValue(0.f , 0.f , 0.f ); - - for(int i=0; i < 3; i++) - { - m_enableMotor[i] = false; - m_servoMotor[i] = false; - m_enableSpring[i] = false; - m_servoTarget[i] = btScalar(0.f); - m_springStiffness[i] = btScalar(0.f); - m_springStiffnessLimited[i] = false; - m_springDamping[i] = btScalar(0.f); - m_springDampingLimited[i] = false; - m_equilibriumPoint[i] = btScalar(0.f); - m_targetVelocity[i] = btScalar(0.f); - m_maxMotorForce[i] = btScalar(0.f); - - m_currentLimit[i] = 0; - } - } - - btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other ) - { - m_lowerLimit = other.m_lowerLimit; - m_upperLimit = other.m_upperLimit; - m_bounce = other.m_bounce; - m_stopERP = other.m_stopERP; - m_stopCFM = other.m_stopCFM; - m_motorERP = other.m_motorERP; - m_motorCFM = other.m_motorCFM; - - m_currentLimitError = other.m_currentLimitError; - m_currentLimitErrorHi = other.m_currentLimitErrorHi; - m_currentLinearDiff = other.m_currentLinearDiff; - - for(int i=0; i < 3; i++) - { - m_enableMotor[i] = other.m_enableMotor[i]; - m_servoMotor[i] = other.m_servoMotor[i]; - m_enableSpring[i] = other.m_enableSpring[i]; - m_servoTarget[i] = other.m_servoTarget[i]; - m_springStiffness[i] = other.m_springStiffness[i]; - m_springStiffnessLimited[i] = other.m_springStiffnessLimited[i]; - m_springDamping[i] = other.m_springDamping[i]; - m_springDampingLimited[i] = other.m_springDampingLimited[i]; - m_equilibriumPoint[i] = other.m_equilibriumPoint[i]; - m_targetVelocity[i] = other.m_targetVelocity[i]; - m_maxMotorForce[i] = other.m_maxMotorForce[i]; - - m_currentLimit[i] = other.m_currentLimit[i]; - } - } - - inline bool isLimited(int limitIndex) - { - return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); - } - - void testLimitValue(int limitIndex, btScalar test_value); -}; - -enum bt6DofFlags2 -{ - BT_6DOF_FLAGS_CFM_STOP2 = 1, - BT_6DOF_FLAGS_ERP_STOP2 = 2, - BT_6DOF_FLAGS_CFM_MOTO2 = 4, - BT_6DOF_FLAGS_ERP_MOTO2 = 8 -}; -#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis - - -ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint -{ -protected: - - btTransform m_frameInA; - btTransform m_frameInB; - - btJacobianEntry m_jacLinear[3]; - btJacobianEntry m_jacAng[3]; - - btTranslationalLimitMotor2 m_linearLimits; - btRotationalLimitMotor2 m_angularLimits[3]; - - RotateOrder m_rotateOrder; - -protected: - - btTransform m_calculatedTransformA; - btTransform m_calculatedTransformB; - btVector3 m_calculatedAxisAngleDiff; - btVector3 m_calculatedAxis[3]; - btVector3 m_calculatedLinearDiff; - btScalar m_factA; - btScalar m_factB; - bool m_hasStaticBody; - int m_flags; - - btGeneric6DofSpring2Constraint& operator=(btGeneric6DofSpring2Constraint&) - { - btAssert(0); - return *this; - } - - int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); - int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); - - void calculateLinearInfo(); - void calculateAngleInfo(); - void testAngularLimitMotor(int axis_index); - - void calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed); - int get_limit_motor_info2(btRotationalLimitMotor2* 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 rotAllowed = false); - - static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); - static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); - static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz); - static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz); - static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz); - static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz); - static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz); - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ); - btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ); - - virtual void buildJacobian() {} - virtual void getInfo1 (btConstraintInfo1* info); - virtual void getInfo2 (btConstraintInfo2* info); - virtual int calculateSerializeBufferSize() const; - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - btRotationalLimitMotor2* getRotationalLimitMotor(int index) { return &m_angularLimits[index]; } - btTranslationalLimitMotor2* getTranslationalLimitMotor() { return &m_linearLimits; } - - // Calculates the global transform for the joint offset for body A an B, and also calculates the angle differences between the bodies. - void calculateTransforms(const btTransform& transA,const btTransform& transB); - void calculateTransforms(); - - // Gets the global transform of the offset for body A - const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } - // Gets the global transform of the offset for body B - const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; } - - const btTransform & getFrameOffsetA() const { return m_frameInA; } - const btTransform & getFrameOffsetB() const { return m_frameInB; } - - btTransform & getFrameOffsetA() { return m_frameInA; } - btTransform & getFrameOffsetB() { return m_frameInB; } - - // Get the rotation axis in global coordinates ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously ) - btVector3 getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; } - - // Get the relative Euler angle ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously ) - btScalar getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; } - - // Get the relative position of the constraint pivot ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously ) - btScalar getRelativePivotPosition(int axis_index) const { return m_calculatedLinearDiff[axis_index]; } - - void setFrames(const btTransform & frameA, const btTransform & frameB); - - void setLinearLowerLimit(const btVector3& linearLower) { m_linearLimits.m_lowerLimit = linearLower; } - void getLinearLowerLimit(btVector3& linearLower) { linearLower = m_linearLimits.m_lowerLimit; } - void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; } - void getLinearUpperLimit(btVector3& linearUpper) { linearUpper = m_linearLimits.m_upperLimit; } - - void setAngularLowerLimit(const btVector3& angularLower) - { - for(int i = 0; i < 3; i++) - m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); - } - - void setAngularLowerLimitReversed(const btVector3& angularLower) - { - for(int i = 0; i < 3; i++) - m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]); - } - - void getAngularLowerLimit(btVector3& angularLower) - { - for(int i = 0; i < 3; i++) - angularLower[i] = m_angularLimits[i].m_loLimit; - } - - void getAngularLowerLimitReversed(btVector3& angularLower) - { - for(int i = 0; i < 3; i++) - angularLower[i] = -m_angularLimits[i].m_hiLimit; - } - - void setAngularUpperLimit(const btVector3& angularUpper) - { - for(int i = 0; i < 3; i++) - m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); - } - - void setAngularUpperLimitReversed(const btVector3& angularUpper) - { - for(int i = 0; i < 3; i++) - m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]); - } - - void getAngularUpperLimit(btVector3& angularUpper) - { - for(int i = 0; i < 3; i++) - angularUpper[i] = m_angularLimits[i].m_hiLimit; - } - - void getAngularUpperLimitReversed(btVector3& angularUpper) - { - for(int i = 0; i < 3; i++) - angularUpper[i] = -m_angularLimits[i].m_loLimit; - } - - //first 3 are linear, next 3 are angular - - void setLimit(int axis, btScalar lo, btScalar hi) - { - if(axis<3) - { - m_linearLimits.m_lowerLimit[axis] = lo; - m_linearLimits.m_upperLimit[axis] = hi; - } - else - { - lo = btNormalizeAngle(lo); - hi = btNormalizeAngle(hi); - m_angularLimits[axis-3].m_loLimit = lo; - m_angularLimits[axis-3].m_hiLimit = hi; - } - } - - void setLimitReversed(int axis, btScalar lo, btScalar hi) - { - if(axis<3) - { - m_linearLimits.m_lowerLimit[axis] = lo; - m_linearLimits.m_upperLimit[axis] = hi; - } - else - { - lo = btNormalizeAngle(lo); - hi = btNormalizeAngle(hi); - m_angularLimits[axis-3].m_hiLimit = -lo; - m_angularLimits[axis-3].m_loLimit = -hi; - } - } - - bool isLimited(int limitIndex) - { - if(limitIndex<3) - { - return m_linearLimits.isLimited(limitIndex); - } - return m_angularLimits[limitIndex-3].isLimited(); - } - - void setRotationOrder(RotateOrder order) { m_rotateOrder = order; } - RotateOrder getRotationOrder() { return m_rotateOrder; } - - void setAxis( const btVector3& axis1, const btVector3& axis2); - - void setBounce(int index, btScalar bounce); - - void enableMotor(int index, bool onOff); - void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also) - void setTargetVelocity(int index, btScalar velocity); - void setServoTarget(int index, btScalar target); - void setMaxMotorForce(int index, btScalar force); - - void enableSpring(int index, bool onOff); - void setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely - void setDamping(int index, btScalar damping, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up - void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF - void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF - void setEquilibriumPoint(int index, btScalar val); - - //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). - //If no axis is provided, it uses the default axis for this constraint. - virtual void setParam(int num, btScalar value, int axis = -1); - virtual btScalar getParam(int num, int axis = -1) const; -}; - - -struct btGeneric6DofSpring2ConstraintData -{ - btTypedConstraintData m_typeConstraintData; - btTransformFloatData m_rbAFrame; - btTransformFloatData m_rbBFrame; - - btVector3FloatData m_linearUpperLimit; - btVector3FloatData m_linearLowerLimit; - btVector3FloatData m_linearBounce; - btVector3FloatData m_linearStopERP; - btVector3FloatData m_linearStopCFM; - btVector3FloatData m_linearMotorERP; - btVector3FloatData m_linearMotorCFM; - btVector3FloatData m_linearTargetVelocity; - btVector3FloatData m_linearMaxMotorForce; - btVector3FloatData m_linearServoTarget; - btVector3FloatData m_linearSpringStiffness; - btVector3FloatData m_linearSpringDamping; - btVector3FloatData m_linearEquilibriumPoint; - char m_linearEnableMotor[4]; - char m_linearServoMotor[4]; - char m_linearEnableSpring[4]; - char m_linearSpringStiffnessLimited[4]; - char m_linearSpringDampingLimited[4]; - char m_padding1[4]; - - btVector3FloatData m_angularUpperLimit; - btVector3FloatData m_angularLowerLimit; - btVector3FloatData m_angularBounce; - btVector3FloatData m_angularStopERP; - btVector3FloatData m_angularStopCFM; - btVector3FloatData m_angularMotorERP; - btVector3FloatData m_angularMotorCFM; - btVector3FloatData m_angularTargetVelocity; - btVector3FloatData m_angularMaxMotorForce; - btVector3FloatData m_angularServoTarget; - btVector3FloatData m_angularSpringStiffness; - btVector3FloatData m_angularSpringDamping; - btVector3FloatData m_angularEquilibriumPoint; - char m_angularEnableMotor[4]; - char m_angularServoMotor[4]; - char m_angularEnableSpring[4]; - char m_angularSpringStiffnessLimited[4]; - char m_angularSpringDampingLimited[4]; - - int m_rotateOrder; -}; - -struct btGeneric6DofSpring2ConstraintDoubleData2 -{ - btTypedConstraintDoubleData m_typeConstraintData; - btTransformDoubleData m_rbAFrame; - btTransformDoubleData m_rbBFrame; - - btVector3DoubleData m_linearUpperLimit; - btVector3DoubleData m_linearLowerLimit; - btVector3DoubleData m_linearBounce; - btVector3DoubleData m_linearStopERP; - btVector3DoubleData m_linearStopCFM; - btVector3DoubleData m_linearMotorERP; - btVector3DoubleData m_linearMotorCFM; - btVector3DoubleData m_linearTargetVelocity; - btVector3DoubleData m_linearMaxMotorForce; - btVector3DoubleData m_linearServoTarget; - btVector3DoubleData m_linearSpringStiffness; - btVector3DoubleData m_linearSpringDamping; - btVector3DoubleData m_linearEquilibriumPoint; - char m_linearEnableMotor[4]; - char m_linearServoMotor[4]; - char m_linearEnableSpring[4]; - char m_linearSpringStiffnessLimited[4]; - char m_linearSpringDampingLimited[4]; - char m_padding1[4]; - - btVector3DoubleData m_angularUpperLimit; - btVector3DoubleData m_angularLowerLimit; - btVector3DoubleData m_angularBounce; - btVector3DoubleData m_angularStopERP; - btVector3DoubleData m_angularStopCFM; - btVector3DoubleData m_angularMotorERP; - btVector3DoubleData m_angularMotorCFM; - btVector3DoubleData m_angularTargetVelocity; - btVector3DoubleData m_angularMaxMotorForce; - btVector3DoubleData m_angularServoTarget; - btVector3DoubleData m_angularSpringStiffness; - btVector3DoubleData m_angularSpringDamping; - btVector3DoubleData m_angularEquilibriumPoint; - char m_angularEnableMotor[4]; - char m_angularServoMotor[4]; - char m_angularEnableSpring[4]; - char m_angularSpringStiffnessLimited[4]; - char m_angularSpringDampingLimited[4]; - - int m_rotateOrder; -}; - -SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const -{ - return sizeof(btGeneric6DofSpring2ConstraintData2); -} - -SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const -{ - btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer; - btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); - - m_frameInA.serialize(dof->m_rbAFrame); - m_frameInB.serialize(dof->m_rbBFrame); - - int i; - for (i=0;i<3;i++) - { - dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit; - dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit; - dof->m_angularBounce.m_floats[i] = m_angularLimits[i].m_bounce; - dof->m_angularStopERP.m_floats[i] = m_angularLimits[i].m_stopERP; - dof->m_angularStopCFM.m_floats[i] = m_angularLimits[i].m_stopCFM; - dof->m_angularMotorERP.m_floats[i] = m_angularLimits[i].m_motorERP; - dof->m_angularMotorCFM.m_floats[i] = m_angularLimits[i].m_motorCFM; - dof->m_angularTargetVelocity.m_floats[i] = m_angularLimits[i].m_targetVelocity; - dof->m_angularMaxMotorForce.m_floats[i] = m_angularLimits[i].m_maxMotorForce; - dof->m_angularServoTarget.m_floats[i] = m_angularLimits[i].m_servoTarget; - dof->m_angularSpringStiffness.m_floats[i] = m_angularLimits[i].m_springStiffness; - dof->m_angularSpringDamping.m_floats[i] = m_angularLimits[i].m_springDamping; - dof->m_angularEquilibriumPoint.m_floats[i] = m_angularLimits[i].m_equilibriumPoint; - } - dof->m_angularLowerLimit.m_floats[3] = 0; - dof->m_angularUpperLimit.m_floats[3] = 0; - dof->m_angularBounce.m_floats[3] = 0; - dof->m_angularStopERP.m_floats[3] = 0; - dof->m_angularStopCFM.m_floats[3] = 0; - dof->m_angularMotorERP.m_floats[3] = 0; - dof->m_angularMotorCFM.m_floats[3] = 0; - dof->m_angularTargetVelocity.m_floats[3] = 0; - dof->m_angularMaxMotorForce.m_floats[3] = 0; - dof->m_angularServoTarget.m_floats[3] = 0; - dof->m_angularSpringStiffness.m_floats[3] = 0; - dof->m_angularSpringDamping.m_floats[3] = 0; - dof->m_angularEquilibriumPoint.m_floats[3] = 0; - for (i=0;i<4;i++) - { - dof->m_angularEnableMotor[i] = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0; - dof->m_angularServoMotor[i] = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0; - dof->m_angularEnableSpring[i] = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0; - dof->m_angularSpringStiffnessLimited[i] = i < 3 ? ( m_angularLimits[i].m_springStiffnessLimited ? 1 : 0 ) : 0; - dof->m_angularSpringDampingLimited[i] = i < 3 ? ( m_angularLimits[i].m_springDampingLimited ? 1 : 0 ) : 0; - } - - m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit ); - m_linearLimits.m_upperLimit.serialize( dof->m_linearUpperLimit ); - m_linearLimits.m_bounce.serialize( dof->m_linearBounce ); - m_linearLimits.m_stopERP.serialize( dof->m_linearStopERP ); - m_linearLimits.m_stopCFM.serialize( dof->m_linearStopCFM ); - m_linearLimits.m_motorERP.serialize( dof->m_linearMotorERP ); - m_linearLimits.m_motorCFM.serialize( dof->m_linearMotorCFM ); - m_linearLimits.m_targetVelocity.serialize( dof->m_linearTargetVelocity ); - m_linearLimits.m_maxMotorForce.serialize( dof->m_linearMaxMotorForce ); - m_linearLimits.m_servoTarget.serialize( dof->m_linearServoTarget ); - m_linearLimits.m_springStiffness.serialize( dof->m_linearSpringStiffness ); - m_linearLimits.m_springDamping.serialize( dof->m_linearSpringDamping ); - m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint ); - for (i=0;i<4;i++) - { - dof->m_linearEnableMotor[i] = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0; - dof->m_linearServoMotor[i] = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0; - dof->m_linearEnableSpring[i] = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0; - dof->m_linearSpringStiffnessLimited[i] = i < 3 ? ( m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0 ) : 0; - dof->m_linearSpringDampingLimited[i] = i < 3 ? ( m_linearLimits.m_springDampingLimited[i] ? 1 : 0 ) : 0; - } - - dof->m_rotateOrder = m_rotateOrder; - - return btGeneric6DofSpring2ConstraintDataName; -} - - - - - -#endif //BT_GENERIC_6DOF_CONSTRAINT_H +/* +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. +*/ + +/* +2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer +Pros: +- Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled) +- Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring) +- Servo motor functionality +- Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision) +- Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2) + +Cons: +- It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. +- At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.) +*/ + +/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev +/// Added support for generic constraint solver through getInfo1/getInfo2 methods + +/* +2007-09-09 +btGeneric6DofConstraint Refactored by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + + +#ifndef BT_GENERIC_6DOF_CONSTRAINT2_H +#define BT_GENERIC_6DOF_CONSTRAINT2_H + +#include "LinearMath/btVector3.h" +#include "btJacobianEntry.h" +#include "btTypedConstraint.h" + +class btRigidBody; + + +#ifdef BT_USE_DOUBLE_PRECISION +#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintDoubleData2 +#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintDoubleData2" +#else +#define btGeneric6DofSpring2ConstraintData2 btGeneric6DofSpring2ConstraintData +#define btGeneric6DofSpring2ConstraintDataName "btGeneric6DofSpring2ConstraintData" +#endif //BT_USE_DOUBLE_PRECISION + +enum RotateOrder +{ + RO_XYZ=0, + RO_XZY, + RO_YXZ, + RO_YZX, + RO_ZXY, + RO_ZYX +}; + +class btRotationalLimitMotor2 +{ +public: +// upper < lower means free +// upper == lower means locked +// upper > lower means limited + btScalar m_loLimit; + btScalar m_hiLimit; + btScalar m_bounce; + btScalar m_stopERP; + btScalar m_stopCFM; + btScalar m_motorERP; + btScalar m_motorCFM; + bool m_enableMotor; + btScalar m_targetVelocity; + btScalar m_maxMotorForce; + bool m_servoMotor; + btScalar m_servoTarget; + bool m_enableSpring; + btScalar m_springStiffness; + bool m_springStiffnessLimited; + btScalar m_springDamping; + bool m_springDampingLimited; + btScalar m_equilibriumPoint; + + btScalar m_currentLimitError; + btScalar m_currentLimitErrorHi; + btScalar m_currentPosition; + int m_currentLimit; + + btRotationalLimitMotor2() + { + m_loLimit = 1.0f; + m_hiLimit = -1.0f; + m_bounce = 0.0f; + m_stopERP = 0.2f; + m_stopCFM = 0.f; + m_motorERP = 0.9f; + m_motorCFM = 0.f; + m_enableMotor = false; + m_targetVelocity = 0; + m_maxMotorForce = 6.0f; + m_servoMotor = false; + m_servoTarget = 0; + m_enableSpring = false; + m_springStiffness = 0; + m_springStiffnessLimited = false; + m_springDamping = 0; + m_springDampingLimited = false; + m_equilibriumPoint = 0; + + m_currentLimitError = 0; + m_currentLimitErrorHi = 0; + m_currentPosition = 0; + m_currentLimit = 0; + } + + btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot) + { + m_loLimit = limot.m_loLimit; + m_hiLimit = limot.m_hiLimit; + m_bounce = limot.m_bounce; + m_stopERP = limot.m_stopERP; + m_stopCFM = limot.m_stopCFM; + m_motorERP = limot.m_motorERP; + m_motorCFM = limot.m_motorCFM; + m_enableMotor = limot.m_enableMotor; + m_targetVelocity = limot.m_targetVelocity; + m_maxMotorForce = limot.m_maxMotorForce; + m_servoMotor = limot.m_servoMotor; + m_servoTarget = limot.m_servoTarget; + m_enableSpring = limot.m_enableSpring; + m_springStiffness = limot.m_springStiffness; + m_springStiffnessLimited = limot.m_springStiffnessLimited; + m_springDamping = limot.m_springDamping; + m_springDampingLimited = limot.m_springDampingLimited; + m_equilibriumPoint = limot.m_equilibriumPoint; + + m_currentLimitError = limot.m_currentLimitError; + m_currentLimitErrorHi = limot.m_currentLimitErrorHi; + m_currentPosition = limot.m_currentPosition; + m_currentLimit = limot.m_currentLimit; + } + + + bool isLimited() + { + if(m_loLimit > m_hiLimit) return false; + return true; + } + + void testLimitValue(btScalar test_value); +}; + + + +class btTranslationalLimitMotor2 +{ +public: +// upper < lower means free +// upper == lower means locked +// upper > lower means limited + btVector3 m_lowerLimit; + btVector3 m_upperLimit; + btVector3 m_bounce; + btVector3 m_stopERP; + btVector3 m_stopCFM; + btVector3 m_motorERP; + btVector3 m_motorCFM; + bool m_enableMotor[3]; + bool m_servoMotor[3]; + bool m_enableSpring[3]; + btVector3 m_servoTarget; + btVector3 m_springStiffness; + bool m_springStiffnessLimited[3]; + btVector3 m_springDamping; + bool m_springDampingLimited[3]; + btVector3 m_equilibriumPoint; + btVector3 m_targetVelocity; + btVector3 m_maxMotorForce; + + btVector3 m_currentLimitError; + btVector3 m_currentLimitErrorHi; + btVector3 m_currentLinearDiff; + int m_currentLimit[3]; + + btTranslationalLimitMotor2() + { + m_lowerLimit .setValue(0.f , 0.f , 0.f ); + m_upperLimit .setValue(0.f , 0.f , 0.f ); + m_bounce .setValue(0.f , 0.f , 0.f ); + m_stopERP .setValue(0.2f, 0.2f, 0.2f); + m_stopCFM .setValue(0.f , 0.f , 0.f ); + m_motorERP .setValue(0.9f, 0.9f, 0.9f); + m_motorCFM .setValue(0.f , 0.f , 0.f ); + + m_currentLimitError .setValue(0.f , 0.f , 0.f ); + m_currentLimitErrorHi.setValue(0.f , 0.f , 0.f ); + m_currentLinearDiff .setValue(0.f , 0.f , 0.f ); + + for(int i=0; i < 3; i++) + { + m_enableMotor[i] = false; + m_servoMotor[i] = false; + m_enableSpring[i] = false; + m_servoTarget[i] = btScalar(0.f); + m_springStiffness[i] = btScalar(0.f); + m_springStiffnessLimited[i] = false; + m_springDamping[i] = btScalar(0.f); + m_springDampingLimited[i] = false; + m_equilibriumPoint[i] = btScalar(0.f); + m_targetVelocity[i] = btScalar(0.f); + m_maxMotorForce[i] = btScalar(0.f); + + m_currentLimit[i] = 0; + } + } + + btTranslationalLimitMotor2(const btTranslationalLimitMotor2 & other ) + { + m_lowerLimit = other.m_lowerLimit; + m_upperLimit = other.m_upperLimit; + m_bounce = other.m_bounce; + m_stopERP = other.m_stopERP; + m_stopCFM = other.m_stopCFM; + m_motorERP = other.m_motorERP; + m_motorCFM = other.m_motorCFM; + + m_currentLimitError = other.m_currentLimitError; + m_currentLimitErrorHi = other.m_currentLimitErrorHi; + m_currentLinearDiff = other.m_currentLinearDiff; + + for(int i=0; i < 3; i++) + { + m_enableMotor[i] = other.m_enableMotor[i]; + m_servoMotor[i] = other.m_servoMotor[i]; + m_enableSpring[i] = other.m_enableSpring[i]; + m_servoTarget[i] = other.m_servoTarget[i]; + m_springStiffness[i] = other.m_springStiffness[i]; + m_springStiffnessLimited[i] = other.m_springStiffnessLimited[i]; + m_springDamping[i] = other.m_springDamping[i]; + m_springDampingLimited[i] = other.m_springDampingLimited[i]; + m_equilibriumPoint[i] = other.m_equilibriumPoint[i]; + m_targetVelocity[i] = other.m_targetVelocity[i]; + m_maxMotorForce[i] = other.m_maxMotorForce[i]; + + m_currentLimit[i] = other.m_currentLimit[i]; + } + } + + inline bool isLimited(int limitIndex) + { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + + void testLimitValue(int limitIndex, btScalar test_value); +}; + +enum bt6DofFlags2 +{ + BT_6DOF_FLAGS_CFM_STOP2 = 1, + BT_6DOF_FLAGS_ERP_STOP2 = 2, + BT_6DOF_FLAGS_CFM_MOTO2 = 4, + BT_6DOF_FLAGS_ERP_MOTO2 = 8 +}; +#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis + + +ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpring2Constraint : public btTypedConstraint +{ +protected: + + btTransform m_frameInA; + btTransform m_frameInB; + + btJacobianEntry m_jacLinear[3]; + btJacobianEntry m_jacAng[3]; + + btTranslationalLimitMotor2 m_linearLimits; + btRotationalLimitMotor2 m_angularLimits[3]; + + RotateOrder m_rotateOrder; + +protected: + + btTransform m_calculatedTransformA; + btTransform m_calculatedTransformB; + btVector3 m_calculatedAxisAngleDiff; + btVector3 m_calculatedAxis[3]; + btVector3 m_calculatedLinearDiff; + btScalar m_factA; + btScalar m_factB; + bool m_hasStaticBody; + int m_flags; + + btGeneric6DofSpring2Constraint& operator=(btGeneric6DofSpring2Constraint&) + { + btAssert(0); + return *this; + } + + int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); + int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); + + void calculateLinearInfo(); + void calculateAngleInfo(); + void testAngularLimitMotor(int axis_index); + + void calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed); + int get_limit_motor_info2(btRotationalLimitMotor2* 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 rotAllowed = false); + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btGeneric6DofSpring2Constraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ); + btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder = RO_XYZ); + + virtual void buildJacobian() {} + virtual void getInfo1 (btConstraintInfo1* info); + virtual void getInfo2 (btConstraintInfo2* info); + virtual int calculateSerializeBufferSize() const; + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; + + btRotationalLimitMotor2* getRotationalLimitMotor(int index) { return &m_angularLimits[index]; } + btTranslationalLimitMotor2* getTranslationalLimitMotor() { return &m_linearLimits; } + + // Calculates the global transform for the joint offset for body A an B, and also calculates the angle differences between the bodies. + void calculateTransforms(const btTransform& transA,const btTransform& transB); + void calculateTransforms(); + + // Gets the global transform of the offset for body A + const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } + // Gets the global transform of the offset for body B + const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; } + + const btTransform & getFrameOffsetA() const { return m_frameInA; } + const btTransform & getFrameOffsetB() const { return m_frameInB; } + + btTransform & getFrameOffsetA() { return m_frameInA; } + btTransform & getFrameOffsetB() { return m_frameInB; } + + // Get the rotation axis in global coordinates ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously ) + btVector3 getAxis(int axis_index) const { return m_calculatedAxis[axis_index]; } + + // Get the relative Euler angle ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously ) + btScalar getAngle(int axis_index) const { return m_calculatedAxisAngleDiff[axis_index]; } + + // Get the relative position of the constraint pivot ( btGeneric6DofSpring2Constraint::calculateTransforms() must be called previously ) + btScalar getRelativePivotPosition(int axis_index) const { return m_calculatedLinearDiff[axis_index]; } + + void setFrames(const btTransform & frameA, const btTransform & frameB); + + void setLinearLowerLimit(const btVector3& linearLower) { m_linearLimits.m_lowerLimit = linearLower; } + void getLinearLowerLimit(btVector3& linearLower) { linearLower = m_linearLimits.m_lowerLimit; } + void setLinearUpperLimit(const btVector3& linearUpper) { m_linearLimits.m_upperLimit = linearUpper; } + void getLinearUpperLimit(btVector3& linearUpper) { linearUpper = m_linearLimits.m_upperLimit; } + + void setAngularLowerLimit(const btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); + } + + void setAngularLowerLimitReversed(const btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_hiLimit = btNormalizeAngle(-angularLower[i]); + } + + void getAngularLowerLimit(btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + angularLower[i] = m_angularLimits[i].m_loLimit; + } + + void getAngularLowerLimitReversed(btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + angularLower[i] = -m_angularLimits[i].m_hiLimit; + } + + void setAngularUpperLimit(const btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); + } + + void setAngularUpperLimitReversed(const btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_loLimit = btNormalizeAngle(-angularUpper[i]); + } + + void getAngularUpperLimit(btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + angularUpper[i] = m_angularLimits[i].m_hiLimit; + } + + void getAngularUpperLimitReversed(btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + angularUpper[i] = -m_angularLimits[i].m_loLimit; + } + + //first 3 are linear, next 3 are angular + + void setLimit(int axis, btScalar lo, btScalar hi) + { + if(axis<3) + { + m_linearLimits.m_lowerLimit[axis] = lo; + m_linearLimits.m_upperLimit[axis] = hi; + } + else + { + lo = btNormalizeAngle(lo); + hi = btNormalizeAngle(hi); + m_angularLimits[axis-3].m_loLimit = lo; + m_angularLimits[axis-3].m_hiLimit = hi; + } + } + + void setLimitReversed(int axis, btScalar lo, btScalar hi) + { + if(axis<3) + { + m_linearLimits.m_lowerLimit[axis] = lo; + m_linearLimits.m_upperLimit[axis] = hi; + } + else + { + lo = btNormalizeAngle(lo); + hi = btNormalizeAngle(hi); + m_angularLimits[axis-3].m_hiLimit = -lo; + m_angularLimits[axis-3].m_loLimit = -hi; + } + } + + bool isLimited(int limitIndex) + { + if(limitIndex<3) + { + return m_linearLimits.isLimited(limitIndex); + } + return m_angularLimits[limitIndex-3].isLimited(); + } + + void setRotationOrder(RotateOrder order) { m_rotateOrder = order; } + RotateOrder getRotationOrder() { return m_rotateOrder; } + + void setAxis( const btVector3& axis1, const btVector3& axis2); + + void setBounce(int index, btScalar bounce); + + void enableMotor(int index, bool onOff); + void setServo(int index, bool onOff); // set the type of the motor (servo or not) (the motor has to be turned on for servo also) + void setTargetVelocity(int index, btScalar velocity); + void setServoTarget(int index, btScalar target); + void setMaxMotorForce(int index, btScalar force); + + void enableSpring(int index, bool onOff); + void setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely + void setDamping(int index, btScalar damping, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up + void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF + void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF + void setEquilibriumPoint(int index, btScalar val); + + //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). + //If no axis is provided, it uses the default axis for this constraint. + virtual void setParam(int num, btScalar value, int axis = -1); + virtual btScalar getParam(int num, int axis = -1) const; + + static btScalar btGetMatrixElem(const btMatrix3x3& mat, int index); + static bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerXZY(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerYXZ(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerYZX(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerZXY(const btMatrix3x3& mat,btVector3& xyz); + static bool matrixToEulerZYX(const btMatrix3x3& mat,btVector3& xyz); +}; + + +struct btGeneric6DofSpring2ConstraintData +{ + btTypedConstraintData m_typeConstraintData; + btTransformFloatData m_rbAFrame; + btTransformFloatData m_rbBFrame; + + btVector3FloatData m_linearUpperLimit; + btVector3FloatData m_linearLowerLimit; + btVector3FloatData m_linearBounce; + btVector3FloatData m_linearStopERP; + btVector3FloatData m_linearStopCFM; + btVector3FloatData m_linearMotorERP; + btVector3FloatData m_linearMotorCFM; + btVector3FloatData m_linearTargetVelocity; + btVector3FloatData m_linearMaxMotorForce; + btVector3FloatData m_linearServoTarget; + btVector3FloatData m_linearSpringStiffness; + btVector3FloatData m_linearSpringDamping; + btVector3FloatData m_linearEquilibriumPoint; + char m_linearEnableMotor[4]; + char m_linearServoMotor[4]; + char m_linearEnableSpring[4]; + char m_linearSpringStiffnessLimited[4]; + char m_linearSpringDampingLimited[4]; + char m_padding1[4]; + + btVector3FloatData m_angularUpperLimit; + btVector3FloatData m_angularLowerLimit; + btVector3FloatData m_angularBounce; + btVector3FloatData m_angularStopERP; + btVector3FloatData m_angularStopCFM; + btVector3FloatData m_angularMotorERP; + btVector3FloatData m_angularMotorCFM; + btVector3FloatData m_angularTargetVelocity; + btVector3FloatData m_angularMaxMotorForce; + btVector3FloatData m_angularServoTarget; + btVector3FloatData m_angularSpringStiffness; + btVector3FloatData m_angularSpringDamping; + btVector3FloatData m_angularEquilibriumPoint; + char m_angularEnableMotor[4]; + char m_angularServoMotor[4]; + char m_angularEnableSpring[4]; + char m_angularSpringStiffnessLimited[4]; + char m_angularSpringDampingLimited[4]; + + int m_rotateOrder; +}; + +struct btGeneric6DofSpring2ConstraintDoubleData2 +{ + btTypedConstraintDoubleData m_typeConstraintData; + btTransformDoubleData m_rbAFrame; + btTransformDoubleData m_rbBFrame; + + btVector3DoubleData m_linearUpperLimit; + btVector3DoubleData m_linearLowerLimit; + btVector3DoubleData m_linearBounce; + btVector3DoubleData m_linearStopERP; + btVector3DoubleData m_linearStopCFM; + btVector3DoubleData m_linearMotorERP; + btVector3DoubleData m_linearMotorCFM; + btVector3DoubleData m_linearTargetVelocity; + btVector3DoubleData m_linearMaxMotorForce; + btVector3DoubleData m_linearServoTarget; + btVector3DoubleData m_linearSpringStiffness; + btVector3DoubleData m_linearSpringDamping; + btVector3DoubleData m_linearEquilibriumPoint; + char m_linearEnableMotor[4]; + char m_linearServoMotor[4]; + char m_linearEnableSpring[4]; + char m_linearSpringStiffnessLimited[4]; + char m_linearSpringDampingLimited[4]; + char m_padding1[4]; + + btVector3DoubleData m_angularUpperLimit; + btVector3DoubleData m_angularLowerLimit; + btVector3DoubleData m_angularBounce; + btVector3DoubleData m_angularStopERP; + btVector3DoubleData m_angularStopCFM; + btVector3DoubleData m_angularMotorERP; + btVector3DoubleData m_angularMotorCFM; + btVector3DoubleData m_angularTargetVelocity; + btVector3DoubleData m_angularMaxMotorForce; + btVector3DoubleData m_angularServoTarget; + btVector3DoubleData m_angularSpringStiffness; + btVector3DoubleData m_angularSpringDamping; + btVector3DoubleData m_angularEquilibriumPoint; + char m_angularEnableMotor[4]; + char m_angularServoMotor[4]; + char m_angularEnableSpring[4]; + char m_angularSpringStiffnessLimited[4]; + char m_angularSpringDampingLimited[4]; + + int m_rotateOrder; +}; + +SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const +{ + return sizeof(btGeneric6DofSpring2ConstraintData2); +} + +SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + btGeneric6DofSpring2ConstraintData2* dof = (btGeneric6DofSpring2ConstraintData2*)dataBuffer; + btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); + + m_frameInA.serialize(dof->m_rbAFrame); + m_frameInB.serialize(dof->m_rbBFrame); + + int i; + for (i=0;i<3;i++) + { + dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit; + dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit; + dof->m_angularBounce.m_floats[i] = m_angularLimits[i].m_bounce; + dof->m_angularStopERP.m_floats[i] = m_angularLimits[i].m_stopERP; + dof->m_angularStopCFM.m_floats[i] = m_angularLimits[i].m_stopCFM; + dof->m_angularMotorERP.m_floats[i] = m_angularLimits[i].m_motorERP; + dof->m_angularMotorCFM.m_floats[i] = m_angularLimits[i].m_motorCFM; + dof->m_angularTargetVelocity.m_floats[i] = m_angularLimits[i].m_targetVelocity; + dof->m_angularMaxMotorForce.m_floats[i] = m_angularLimits[i].m_maxMotorForce; + dof->m_angularServoTarget.m_floats[i] = m_angularLimits[i].m_servoTarget; + dof->m_angularSpringStiffness.m_floats[i] = m_angularLimits[i].m_springStiffness; + dof->m_angularSpringDamping.m_floats[i] = m_angularLimits[i].m_springDamping; + dof->m_angularEquilibriumPoint.m_floats[i] = m_angularLimits[i].m_equilibriumPoint; + } + dof->m_angularLowerLimit.m_floats[3] = 0; + dof->m_angularUpperLimit.m_floats[3] = 0; + dof->m_angularBounce.m_floats[3] = 0; + dof->m_angularStopERP.m_floats[3] = 0; + dof->m_angularStopCFM.m_floats[3] = 0; + dof->m_angularMotorERP.m_floats[3] = 0; + dof->m_angularMotorCFM.m_floats[3] = 0; + dof->m_angularTargetVelocity.m_floats[3] = 0; + dof->m_angularMaxMotorForce.m_floats[3] = 0; + dof->m_angularServoTarget.m_floats[3] = 0; + dof->m_angularSpringStiffness.m_floats[3] = 0; + dof->m_angularSpringDamping.m_floats[3] = 0; + dof->m_angularEquilibriumPoint.m_floats[3] = 0; + for (i=0;i<4;i++) + { + dof->m_angularEnableMotor[i] = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0; + dof->m_angularServoMotor[i] = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0; + dof->m_angularEnableSpring[i] = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0; + dof->m_angularSpringStiffnessLimited[i] = i < 3 ? ( m_angularLimits[i].m_springStiffnessLimited ? 1 : 0 ) : 0; + dof->m_angularSpringDampingLimited[i] = i < 3 ? ( m_angularLimits[i].m_springDampingLimited ? 1 : 0 ) : 0; + } + + m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit ); + m_linearLimits.m_upperLimit.serialize( dof->m_linearUpperLimit ); + m_linearLimits.m_bounce.serialize( dof->m_linearBounce ); + m_linearLimits.m_stopERP.serialize( dof->m_linearStopERP ); + m_linearLimits.m_stopCFM.serialize( dof->m_linearStopCFM ); + m_linearLimits.m_motorERP.serialize( dof->m_linearMotorERP ); + m_linearLimits.m_motorCFM.serialize( dof->m_linearMotorCFM ); + m_linearLimits.m_targetVelocity.serialize( dof->m_linearTargetVelocity ); + m_linearLimits.m_maxMotorForce.serialize( dof->m_linearMaxMotorForce ); + m_linearLimits.m_servoTarget.serialize( dof->m_linearServoTarget ); + m_linearLimits.m_springStiffness.serialize( dof->m_linearSpringStiffness ); + m_linearLimits.m_springDamping.serialize( dof->m_linearSpringDamping ); + m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint ); + for (i=0;i<4;i++) + { + dof->m_linearEnableMotor[i] = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0; + dof->m_linearServoMotor[i] = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0; + dof->m_linearEnableSpring[i] = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0; + dof->m_linearSpringStiffnessLimited[i] = i < 3 ? ( m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0 ) : 0; + dof->m_linearSpringDampingLimited[i] = i < 3 ? ( m_linearLimits.m_springDampingLimited[i] ? 1 : 0 ) : 0; + } + + dof->m_rotateOrder = m_rotateOrder; + + dof->m_padding1[0] = 0; + dof->m_padding1[1] = 0; + dof->m_padding1[2] = 0; + dof->m_padding1[3] = 0; + + return btGeneric6DofSpring2ConstraintDataName; +} + + + + + +#endif //BT_GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp similarity index 97% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp index 6f765884ec0a..3f875989ea52 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp @@ -131,7 +131,7 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf 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; + m_linearLimits.m_maxMotorForce[i] = btFabs(force); } } for(i = 0; i < 3; i++) @@ -146,7 +146,7 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf 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; + m_angularLimits[i].m_maxMotorForce = btFabs(force); } } } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp similarity index 99% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 76a1509471e8..7e5e6f9e54cd 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -556,12 +556,8 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf } // if the hinge has joint limits or motor, add in the extra row - int powered = 0; - if(getEnableAngularMotor()) - { - powered = 1; - } - if(limit || powered) + bool powered = getEnableAngularMotor(); + if(limit || powered) { nrow++; srow = nrow * info->rowskip; @@ -577,7 +573,7 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf btScalar histop = getUpperLimit(); if(limit && (lostop == histop)) { // the joint motor is ineffective - powered = 0; + powered = false; } info->m_constraintError[srow] = btScalar(0.0f); btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; @@ -951,12 +947,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info } // if the hinge has joint limits or motor, add in the extra row - int powered = 0; - if(getEnableAngularMotor()) - { - powered = 1; - } - if(limit || powered) + bool powered = getEnableAngularMotor(); + if(limit || powered) { nrow++; srow = nrow * info->rowskip; @@ -972,7 +964,7 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info btScalar histop = getUpperLimit(); if(limit && (lostop == histop)) { // the joint motor is ineffective - powered = 0; + powered = false; } info->m_constraintError[srow] = btScalar(0.0f); btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h similarity index 98% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index f26e72105ba7..3c3df24dba27 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -314,7 +314,7 @@ ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint { return m_enableAngularMotor; } - inline btScalar getMotorTargetVelosity() + inline btScalar getMotorTargetVelocity() { return m_motorTargetVelocity; } @@ -489,6 +489,14 @@ SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btS hingeData->m_relaxationFactor = float(m_relaxationFactor); #endif + // Fill padding with zeros to appease msan. +#ifdef BT_USE_DOUBLE_PRECISION + hingeData->m_padding1[0] = 0; + hingeData->m_padding1[1] = 0; + hingeData->m_padding1[2] = 0; + hingeData->m_padding1[3] = 0; +#endif + return btHingeConstraintDataName; } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp similarity index 74% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp index f110cd480812..f3979be358a5 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp @@ -78,20 +78,6 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision btScalar deltaflengthsqr = 0; - - if (infoGlobal.m_solverMode & SOLVER_SIMD) - { - for (int j=0;jisEnabled()) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } - } - ///solve all contact constraints - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (int j=0;jbtScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - m_deltafCF[j] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCF[j] = 0; - } - } - - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (int j=0;jbtScalar(0)) - { - btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; - if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) - rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - - rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; - rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - - btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); - m_deltafCRF[j] = deltaf; - deltaflengthsqr += deltaf*deltaf; - } else { - m_deltafCRF[j] = 0; - } - } - } } @@ -362,10 +278,7 @@ btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollision for (int j=0;j= m_kinematicBodyUniqueIdToSolverBodyTable.size()) + { + m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID); + } + solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ]; + // if no table entry yet, + if ( solverBodyId == INVALID_SOLVER_BODY_ID ) + { + // create a table entry for this body + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody( &solverBody, &body, timeStep ); + m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ] = solverBodyId; + } + } + else + { + bool isMultiBodyType = (body.getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK); + // Incorrectly set collision object flags can degrade performance in various ways. + if (!isMultiBodyType) + { + btAssert( body.isStaticOrKinematicObject() ); + } + //it could be a multibody link collider + // all fixed bodies (inf mass) get mapped to a single solver id + if ( m_fixedBodyId < 0 ) + { + m_fixedBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody( &fixedBody, 0, timeStep ); + } + solverBodyId = m_fixedBodyId; + } + btAssert( solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size() ); + return solverBodyId; +#else // BT_THREADSAFE - int solverBodyIdA = -1; + int solverBodyIdA = -1; if (body.getCompanionId() >= 0) { @@ -749,6 +833,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& } return solverBodyIdA; +#endif // BT_THREADSAFE } #include @@ -775,7 +860,37 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - relaxation = 1.f; + relaxation = infoGlobal.m_sor; + btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; + + //cfm = 1 / ( dt * kp + kd ) + //erp = dt * kp / ( dt * kp + kd ) + + btScalar cfm = infoGlobal.m_globalCfm; + btScalar erp = infoGlobal.m_erp2; + + if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)) + { + if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) + cfm = cp.m_contactCFM; + if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP) + erp = cp.m_contactERP; + } else + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING) + { + btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 ); + if (denom < SIMD_EPSILON) + { + denom = SIMD_EPSILON; + } + cfm = btScalar(1) / denom; + erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; + } + } + + cfm *= invTimeStep; + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); @@ -802,7 +917,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra } #endif //COMPUTE_IMPULSE_DENOM - btScalar denom = relaxation/(denom0+denom1); + btScalar denom = relaxation/(denom0+denom1+cfm); solverConstraint.m_jacDiagABInv = denom; } @@ -843,7 +958,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra solverConstraint.m_friction = cp.m_combinedFriction; - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); if (restitution <= btScalar(0.)) { restitution = 0.f; @@ -884,20 +999,16 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra btScalar velocityError = restitution - rel_vel;// * damping; - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - + if (penetration>0) { positionalError = 0; - velocityError -= penetration / infoGlobal.m_timeStep; + velocityError -= penetration *invTimeStep; } else { - positionalError = -penetration * erp/infoGlobal.m_timeStep; + positionalError = -penetration * erp*invTimeStep; + } btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; @@ -915,7 +1026,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhsPenetration = penetrationImpulse; } - solverConstraint.m_cfm = 0.f; + solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; } @@ -1010,8 +1121,6 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m int frictionIndex = m_tmpSolverContactConstraintPool.size(); btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; @@ -1023,9 +1132,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); - btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - + btVector3 vel1; + btVector3 vel2; + solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1); solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 ); @@ -1036,45 +1145,31 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m -// const btVector3& pos1 = cp.getPositionWorldOnA(); -// const btVector3& pos2 = cp.getPositionWorldOnB(); /////setup the friction constraints solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); - btVector3 angVelA(0,0,0),angVelB(0,0,0); - if (rb0) - angVelA = rb0->getAngularVelocity(); - if (rb1) - angVelB = rb1->getAngularVelocity(); - btVector3 relAngVel = angVelB-angVelA; - if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) { - //only a single rollingFriction per manifold - rollingFriction--; - if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) - { - relAngVel.normalize(); - applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (relAngVel.length()>0.001) - addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else + { - addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addTorsionalFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,cp.m_combinedSpinningFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation); btVector3 axis0,axis1; btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); + axis0.normalize(); + axis1.normalize(); + applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); if (axis0.length()>0.001) - addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addTorsionalFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp, + cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation); if (axis1.length()>0.001) - addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addTorsionalFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp, + cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } } @@ -1094,7 +1189,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect /// - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) + + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) { cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); @@ -1103,7 +1199,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,infoGlobal); if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { @@ -1111,7 +1207,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m cp.m_lateralFrictionDir2.normalize();//?? applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal); } } else @@ -1120,28 +1216,28 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal); } if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) { - cp.m_lateralFrictionInitialized = true; + cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; } } } else { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); } setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); @@ -1167,12 +1263,270 @@ void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** } } + +void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow, + btTypedConstraint* constraint, + const btTypedConstraint::btConstraintInfo1& info1, + int solverBodyIdA, + int solverBodyIdB, + const btContactSolverInfo& infoGlobal + ) +{ + const btRigidBody& rbA = constraint->getRigidBodyA(); + const btRigidBody& rbB = constraint->getRigidBodyB(); + + const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; + const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; + + int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; + if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) + m_maxOverrideNumSolverIterations = overrideNumSolverIterations; + + for (int j=0;jgetDeltaLinearVelocity().isZero()); + btAssert(bodyAPtr->getDeltaAngularVelocity().isZero()); + btAssert(bodyAPtr->getPushVelocity().isZero()); + btAssert(bodyAPtr->getTurnVelocity().isZero()); + btAssert(bodyBPtr->getDeltaLinearVelocity().isZero()); + btAssert(bodyBPtr->getDeltaAngularVelocity().isZero()); + btAssert(bodyBPtr->getPushVelocity().isZero()); + btAssert(bodyBPtr->getTurnVelocity().isZero()); + //bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + //bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + //bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + //bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + //bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + //bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + //bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + //bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + + + btTypedConstraint::btConstraintInfo2 info2; + info2.fps = 1.f/infoGlobal.m_timeStep; + info2.erp = infoGlobal.m_erp; + info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1; + info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; + info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2; + info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; + info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this + ///the size of btSolverConstraint needs be a multiple of btScalar + btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); + info2.m_constraintError = ¤tConstraintRow->m_rhs; + currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; + info2.m_damping = infoGlobal.m_damping; + info2.cfm = ¤tConstraintRow->m_cfm; + info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; + info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + info2.m_numIterations = infoGlobal.m_numIterations; + constraint->getInfo2(&info2); + + ///finalize the constraint setup + for (int j=0;j=constraint->getBreakingImpulseThreshold()) + { + solverConstraint.m_upperLimit = constraint->getBreakingImpulseThreshold(); + } + + if (solverConstraint.m_lowerLimit<=-constraint->getBreakingImpulseThreshold()) + { + solverConstraint.m_lowerLimit = -constraint->getBreakingImpulseThreshold(); + } + + solverConstraint.m_originalContactPoint = constraint; + + { + const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; + solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); + } + { + const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; + solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); + } + + { + btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass(); + btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; + btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal? + btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; + + btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1); + sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + sum += iMJlB.dot(solverConstraint.m_contactNormal2); + sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); + btScalar fsum = btFabs(sum); + btAssert(fsum > SIMD_EPSILON); + btScalar sorRelaxation = 1.f;//todo: get from globalInfo? + solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?sorRelaxation/sum : 0.f; + } + + { + btScalar rel_vel; + btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0); + btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0); + + btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0); + btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0); + + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) + + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA); + + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) + + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB); + + rel_vel = vel1Dotn+vel2Dotn; + btScalar restitution = 0.f; + btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 + btScalar velocityError = restitution - rel_vel * info2.m_damping; + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_appliedImpulse = 0.f; + } + } +} + + +void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("convertJoints"); + for (int j=0;jbuildJacobian(); + constraint->internalSetAppliedImpulse(0.0f); + } + + int totalNumRows = 0; + + m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); + //calculate the total number of contraint rows + for (int i=0;igetJointFeedback(); + if (fb) + { + fb->m_appliedForceBodyA.setZero(); + fb->m_appliedTorqueBodyA.setZero(); + fb->m_appliedForceBodyB.setZero(); + fb->m_appliedTorqueBodyB.setZero(); + } + + if (constraints[i]->isEnabled()) + { + constraints[i]->getInfo1(&info1); + } else + { + info1.m_numConstraintRows = 0; + info1.nub = 0; + } + totalNumRows += info1.m_numConstraintRows; + } + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); + + + ///setup the btSolverConstraints + int currentRow = 0; + + for (int i=0;igetRigidBodyA(); + btRigidBody& rbB = constraint->getRigidBodyB(); + + int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep); + int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep); + + convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal); + } + currentRow+=info1.m_numConstraintRows; + } +} + + +void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("convertBodies"); + for (int i = 0; i < numBodies; i++) + { + bodies[i]->setCompanionId(-1); + } +#if BT_THREADSAFE + m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 ); +#endif // BT_THREADSAFE + + m_tmpSolverBodyPool.reserve(numBodies+1); + m_tmpSolverBodyPool.resize(0); + + //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + //initSolverBody(&fixedBody,0); + + for (int i=0;igetInvMass()) + { + btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + btVector3 gyroForce (0,0,0); + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) + { + gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce); + solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; + } + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD) + { + gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep); + solverBody.m_externalTorqueImpulse += gyroForce; + } + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY) + { + gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep); + solverBody.m_externalTorqueImpulse += gyroForce; + + } + } + } +} + + btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { m_fixedBodyId = -1; BT_PROFILE("solveGroupCacheFriendlySetup"); (void)debugDrawer; + // if solver mode has changed, + if ( infoGlobal.m_solverMode != m_cachedSolverMode ) + { + // update solver functions to use SIMD or non-SIMD + bool useSimd = !!( infoGlobal.m_solverMode & SOLVER_SIMD ); + setupSolverFunctions( useSimd ); + m_cachedSolverMode = infoGlobal.m_solverMode; + } m_maxOverrideNumSolverIterations = 0; #ifdef BT_ADDITIONAL_DEBUG @@ -1245,250 +1599,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol #endif //BT_ADDITIONAL_DEBUG - for (int i = 0; i < numBodies; i++) - { - bodies[i]->setCompanionId(-1); - } - - - m_tmpSolverBodyPool.reserve(numBodies+1); - m_tmpSolverBodyPool.resize(0); - - //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - //initSolverBody(&fixedBody,0); - //convert all bodies + convertBodies(bodies, numBodies, infoGlobal); + convertJoints(constraints, numConstraints, infoGlobal); - for (int i=0;igetInvMass()) - { - btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; - btVector3 gyroForce (0,0,0); - if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) - { - gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce); - solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; - } - if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD) - { - gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep); - solverBody.m_externalTorqueImpulse += gyroForce; - } - if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY) - { - gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep); - solverBody.m_externalTorqueImpulse += gyroForce; - - } - + convertContacts(manifoldPtr,numManifolds,infoGlobal); - } - } - - if (1) - { - int j; - for (j=0;jbuildJacobian(); - constraint->internalSetAppliedImpulse(0.0f); - } - } - - //btRigidBody* rb0=0,*rb1=0; - - //if (1) - { - { - - int totalNumRows = 0; - int i; - - m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); - //calculate the total number of contraint rows - for (i=0;igetJointFeedback(); - if (fb) - { - fb->m_appliedForceBodyA.setZero(); - fb->m_appliedTorqueBodyA.setZero(); - fb->m_appliedForceBodyB.setZero(); - fb->m_appliedTorqueBodyB.setZero(); - } - - if (constraints[i]->isEnabled()) - { - } - if (constraints[i]->isEnabled()) - { - constraints[i]->getInfo1(&info1); - } else - { - info1.m_numConstraintRows = 0; - info1.nub = 0; - } - totalNumRows += info1.m_numConstraintRows; - } - m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); - - - ///setup the btSolverConstraints - int currentRow = 0; - - for (i=0;igetRigidBodyA(); - btRigidBody& rbB = constraint->getRigidBodyB(); - - int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep); - int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep); - - btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; - - - - - int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; - if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) - m_maxOverrideNumSolverIterations = overrideNumSolverIterations; - - - int j; - for ( j=0;jinternalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - - - btTypedConstraint::btConstraintInfo2 info2; - info2.fps = 1.f/infoGlobal.m_timeStep; - info2.erp = infoGlobal.m_erp; - info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1; - info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; - info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2; - info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; - info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this - ///the size of btSolverConstraint needs be a multiple of btScalar - btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); - info2.m_constraintError = ¤tConstraintRow->m_rhs; - currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; - info2.m_damping = infoGlobal.m_damping; - info2.cfm = ¤tConstraintRow->m_cfm; - info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; - info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; - info2.m_numIterations = infoGlobal.m_numIterations; - constraints[i]->getInfo2(&info2); - - ///finalize the constraint setup - for ( j=0;j=constraints[i]->getBreakingImpulseThreshold()) - { - solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold(); - } - - if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold()) - { - solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold(); - } - - solverConstraint.m_originalContactPoint = constraint; - - { - const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; - solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); - } - { - const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; - solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); - } - - { - btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass(); - btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; - btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal? - btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; - - btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1); - sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); - sum += iMJlB.dot(solverConstraint.m_contactNormal2); - sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - btScalar fsum = btFabs(sum); - btAssert(fsum > SIMD_EPSILON); - solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f; - } - - - - { - btScalar rel_vel; - btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0); - btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0); - - btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0); - btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0); - - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) - + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA); - - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) - + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB); - - rel_vel = vel1Dotn+vel2Dotn; - btScalar restitution = 0.f; - btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 - btScalar velocityError = restitution - rel_vel * info2.m_damping; - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_appliedImpulse = 0.f; - - - } - } - } - currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; - } - } - - convertContacts(manifoldPtr,numManifolds,infoGlobal); - - } // btContactSolverInfo info = infoGlobal; @@ -1528,6 +1645,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/) { + BT_PROFILE("solveSingleIteration"); + btScalar leastSquaresResidual = 0.f; int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); @@ -1565,14 +1684,15 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration } } - if (infoGlobal.m_solverMode & SOLVER_SIMD) - { - ///solve all joint constraints, using SIMD, if available + ///solve all joint constraints for (int j=0;jisEnabled()) - { - int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); - } - } - ///solve all contact constraints - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (int j=0;jbtScalar(0)) - { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - } - } - - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (int j=0;jbtScalar(0)) - { - btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; - if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) - rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - - rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; - rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - - resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); - } - } } - } - return 0.f; + return leastSquaresResidual; } void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { + BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); int iteration; if (infoGlobal.m_splitImpulse) { - if (infoGlobal.m_solverMode & SOLVER_SIMD) { for ( iteration = 0;iteration=(infoGlobal.m_numIterations-1)) { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int j; - for (j=0;j= 0;iteration--) { - solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + m_leastSquaresResidual = solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + + if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration>= (maxIterations-1))) + { +#ifdef VERBOSE_RESIDUAL_PRINTF + printf("residual = %f at iteration #%d\n",m_leastSquaresResidual,iteration); +#endif + break; + } } } return 0.f; } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int i,j; - - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - for (j=0;jsetEnabled(false); } } +} - - for ( i=0;isetCompanionId(-1); } } +} + +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("solveGroupCacheFriendlyFinish"); + + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal); + } + + writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal); + writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal); m_tmpSolverContactConstraintPool.resizeNoInitialize(0); m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h similarity index 70% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index a60291809837..c853aed77c81 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -45,50 +45,70 @@ ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstr btAlignedObjectArray m_tmpConstraintSizesPool; int m_maxOverrideNumSolverIterations; int m_fixedBodyId; + // When running solvers on multiple threads, a race condition exists for Kinematic objects that + // participate in more than one solver. + // The getOrInitSolverBody() function writes the companionId of each body (storing the index of the solver body + // for the current solver). For normal dynamic bodies it isn't an issue because they can only be in one island + // (and therefore one thread) at a time. But kinematic bodies can be in multiple islands at once. + // To avoid this race condition, this solver does not write the companionId, instead it stores the solver body + // index in this solver-local table, indexed by the uniqueId of the body. + btAlignedObjectArray m_kinematicBodyUniqueIdToSolverBodyTable; // only used for multithreading btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric; btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit; + btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse; + int m_cachedSolverMode; // used to check if SOLVER_SIMD flag has been changed + void setupSolverFunctions( bool useSimd ); + + btScalar m_leastSquaresResidual; void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, + const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - void setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, - btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, + void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, + btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - btSolverConstraint& addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f); + btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.); + btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f); + - - void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, + void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2); static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode); - void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, + void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction unsigned long m_btSeed2; - - btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); + + btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold); virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); + virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal); + void convertJoint(btSolverConstraint* currentConstraintRow, btTypedConstraint* constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal); + + virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); - void resolveSplitPenetrationSIMD( - btSolverBody& bodyA,btSolverBody& bodyB, - const btSolverConstraint& contactConstraint); + btSimdScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) + { + return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); + } - void resolveSplitPenetrationImpulseCacheFriendly( - btSolverBody& bodyA,btSolverBody& bodyB, - const btSolverConstraint& contactConstraint); + btSimdScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) + { + return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); + } //internal method int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep); @@ -98,10 +118,16 @@ ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstr btSimdScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); btSimdScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); btSimdScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - + btSimdScalar resolveSplitPenetrationImpulse(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) + { + return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); + } + protected: - - + + void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); @@ -113,15 +139,15 @@ ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstr public: BT_DECLARE_ALIGNED_ALLOCATOR(); - + btSequentialImpulseConstraintSolver(); virtual ~btSequentialImpulseConstraintSolver(); virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher); - + ///clear internal cached data and reset random seed virtual void reset(); - + unsigned long btRand2(); int btRandInt2 (int n); @@ -135,7 +161,7 @@ ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstr return m_btSeed2; } - + virtual btConstraintSolverType getSolverType() const { return BT_SEQUENTIAL_IMPULSE_SOLVER; diff --git a/extern/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp new file mode 100644 index 000000000000..4306c37e495a --- /dev/null +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp @@ -0,0 +1,1621 @@ +/* +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 "btSequentialImpulseConstraintSolverMt.h" + +#include "LinearMath/btQuickprof.h" + +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" + +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + + + +bool btSequentialImpulseConstraintSolverMt::s_allowNestedParallelForLoops = false; // some task schedulers don't like nested loops +int btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching = 250; +int btSequentialImpulseConstraintSolverMt::s_minBatchSize = 50; +int btSequentialImpulseConstraintSolverMt::s_maxBatchSize = 100; +btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_contactBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D; +btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_jointBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D; + + +btSequentialImpulseConstraintSolverMt::btSequentialImpulseConstraintSolverMt() +{ + m_numFrictionDirections = 1; + m_useBatching = false; + m_useObsoleteJointConstraints = false; +} + + +btSequentialImpulseConstraintSolverMt::~btSequentialImpulseConstraintSolverMt() +{ +} + + +void btSequentialImpulseConstraintSolverMt::setupBatchedContactConstraints() +{ + BT_PROFILE("setupBatchedContactConstraints"); + m_batchedContactConstraints.setup( &m_tmpSolverContactConstraintPool, + m_tmpSolverBodyPool, + s_contactBatchingMethod, + s_minBatchSize, + s_maxBatchSize, + &m_scratchMemory + ); +} + + +void btSequentialImpulseConstraintSolverMt::setupBatchedJointConstraints() +{ + BT_PROFILE("setupBatchedJointConstraints"); + m_batchedJointConstraints.setup( &m_tmpSolverNonContactConstraintPool, + m_tmpSolverBodyPool, + s_jointBatchingMethod, + s_minBatchSize, + s_maxBatchSize, + &m_scratchMemory + ); +} + + +void btSequentialImpulseConstraintSolverMt::internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal) +{ + btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[iContactConstraint]; + + btVector3 rel_pos1; + btVector3 rel_pos2; + btScalar relaxation; + + int solverBodyIdA = contactConstraint.m_solverBodyIdA; + int solverBodyIdB = contactConstraint.m_solverBodyIdB; + + btSolverBody* solverBodyA = &m_tmpSolverBodyPool[ solverBodyIdA ]; + btSolverBody* solverBodyB = &m_tmpSolverBodyPool[ solverBodyIdB ]; + + btRigidBody* colObj0 = solverBodyA->m_originalBody; + btRigidBody* colObj1 = solverBodyB->m_originalBody; + + btManifoldPoint& cp = *static_cast( contactConstraint.m_originalContactPoint ); + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + rel_pos1 = pos1 - solverBodyA->getWorldTransform().getOrigin(); + rel_pos2 = pos2 - solverBodyB->getWorldTransform().getOrigin(); + + btVector3 vel1; + btVector3 vel2; + + solverBodyA->getVelocityInLocalPointNoDelta( rel_pos1, vel1 ); + solverBodyB->getVelocityInLocalPointNoDelta( rel_pos2, vel2 ); + + btVector3 vel = vel1 - vel2; + btScalar rel_vel = cp.m_normalWorldOnB.dot( vel ); + + setupContactConstraint( contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2 ); + + // setup rolling friction constraints + int rollingFrictionIndex = m_rollingFrictionIndexTable[iContactConstraint]; + if (rollingFrictionIndex >= 0) + { + btSolverConstraint& spinningFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ rollingFrictionIndex ]; + btAssert( spinningFrictionConstraint.m_frictionIndex == iContactConstraint ); + setupTorsionalFrictionConstraint( spinningFrictionConstraint, + cp.m_normalWorldOnB, + solverBodyIdA, + solverBodyIdB, + cp, + cp.m_combinedSpinningFriction, + rel_pos1, + rel_pos2, + colObj0, + colObj1, + relaxation, + 0.0f, + 0.0f + ); + btVector3 axis[2]; + btPlaneSpace1( cp.m_normalWorldOnB, axis[0], axis[1] ); + axis[0].normalize(); + axis[1].normalize(); + + applyAnisotropicFriction( colObj0, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION ); + applyAnisotropicFriction( colObj1, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION ); + applyAnisotropicFriction( colObj0, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION ); + applyAnisotropicFriction( colObj1, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION ); + // put the largest axis first + if (axis[1].length2() > axis[0].length2()) + { + btSwap(axis[0], axis[1]); + } + const btScalar kRollingFrictionThreshold = 0.001f; + for (int i = 0; i < 2; ++i) + { + int iRollingFric = rollingFrictionIndex + 1 + i; + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ]; + btAssert(rollingFrictionConstraint.m_frictionIndex == iContactConstraint); + btVector3 dir = axis[i]; + if ( dir.length() > kRollingFrictionThreshold ) + { + setupTorsionalFrictionConstraint( rollingFrictionConstraint, + dir, + solverBodyIdA, + solverBodyIdB, + cp, + cp.m_combinedRollingFriction, + rel_pos1, + rel_pos2, + colObj0, + colObj1, + relaxation, + 0.0f, + 0.0f + ); + } + else + { + rollingFrictionConstraint.m_frictionIndex = -1; // disable constraint + } + } + } + + // setup friction constraints + // setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); + { + ///Bullet has several options to set the friction directions + ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///based on the relative linear velocity. + ///If the relative velocity it zero, it will automatically compute a friction direction. + + ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. + ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. + /// + ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. + /// + ///The user can manually override the friction directions for certain contacts using a contact callback, + ///and set the cp.m_lateralFrictionInitialized to true + ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) + ///this will give a conveyor belt effect + /// + btSolverConstraint* frictionConstraint1 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex]; + btAssert(frictionConstraint1->m_frictionIndex == iContactConstraint); + + btSolverConstraint* frictionConstraint2 = NULL; + if ( infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) + { + frictionConstraint2 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex + 1]; + btAssert( frictionConstraint2->m_frictionIndex == iContactConstraint ); + } + + if ( !( infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING ) || !( cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED ) ) + { + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); + if ( !( infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) && lat_rel_vel > SIMD_EPSILON ) + { + cp.m_lateralFrictionDir1 *= 1.f / btSqrt( lat_rel_vel ); + applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal ); + + if ( frictionConstraint2 ) + { + cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross( cp.m_normalWorldOnB ); + cp.m_lateralFrictionDir2.normalize();//?? + applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal ); + } + } + else + { + btPlaneSpace1( cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2 ); + + applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal ); + + if ( frictionConstraint2 ) + { + applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION ); + setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal ); + } + + if ( ( infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) && ( infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) ) + { + cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; + } + } + } + else + { + setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM ); + if ( frictionConstraint2 ) + { + setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM ); + } + } + } + + setFrictionConstraintImpulse( contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal ); +} + + +struct SetupContactConstraintsLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + const btContactSolverInfo* m_infoGlobal; + + SetupContactConstraintsLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, const btContactSolverInfo& infoGlobal ) + { + m_solver = solver; + m_bc = bc; + m_infoGlobal = &infoGlobal; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "SetupContactConstraintsLoop" ); + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + for (int i = batch.begin; i < batch.end; ++i) + { + int iContact = m_bc->m_constraintIndices[i]; + m_solver->internalSetupContactConstraints( iContact, *m_infoGlobal ); + } + } + } +}; + + +void btSequentialImpulseConstraintSolverMt::setupAllContactConstraints(const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE( "setupAllContactConstraints" ); + if ( m_useBatching ) + { + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + SetupContactConstraintsLoop loop( this, &batchedCons, infoGlobal ); + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = 1; + btParallelFor( phase.begin, phase.end, grainSize, loop ); + } + } + else + { + for ( int i = 0; i < m_tmpSolverContactConstraintPool.size(); ++i ) + { + internalSetupContactConstraints( i, infoGlobal ); + } + } +} + + +int btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btCollisionObject& body,btScalar timeStep) +{ + // + // getOrInitSolverBody is threadsafe only for a single thread per solver (with potentially multiple solvers) + // + // getOrInitSolverBodyThreadsafe -- attempts to be fully threadsafe (however may affect determinism) + // + int solverBodyId = -1; + bool isRigidBodyType = btRigidBody::upcast( &body ) != NULL; + if ( isRigidBodyType && !body.isStaticOrKinematicObject() ) + { + // dynamic body + // Dynamic bodies can only be in one island, so it's safe to write to the companionId + solverBodyId = body.getCompanionId(); + if ( solverBodyId < 0 ) + { + m_bodySolverArrayMutex.lock(); + // now that we have the lock, check again + solverBodyId = body.getCompanionId(); + if ( solverBodyId < 0 ) + { + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody( &solverBody, &body, timeStep ); + body.setCompanionId( solverBodyId ); + } + m_bodySolverArrayMutex.unlock(); + } + } + else if (isRigidBodyType && body.isKinematicObject()) + { + // + // NOTE: must test for kinematic before static because some kinematic objects also + // identify as "static" + // + // Kinematic bodies can be in multiple islands at once, so it is a + // race condition to write to them, so we use an alternate method + // to record the solverBodyId + int uniqueId = body.getWorldArrayIndex(); + const int INVALID_SOLVER_BODY_ID = -1; + if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId ) + { + m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock(); + // now that we have the lock, check again + if ( m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId ) + { + m_kinematicBodyUniqueIdToSolverBodyTable.resize( uniqueId + 1, INVALID_SOLVER_BODY_ID ); + } + m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock(); + } + solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ]; + // if no table entry yet, + if ( INVALID_SOLVER_BODY_ID == solverBodyId ) + { + // need to acquire both locks + m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock(); + m_bodySolverArrayMutex.lock(); + // now that we have the lock, check again + solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ]; + if ( INVALID_SOLVER_BODY_ID == solverBodyId ) + { + // create a table entry for this body + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody( &solverBody, &body, timeStep ); + m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ] = solverBodyId; + } + m_bodySolverArrayMutex.unlock(); + m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock(); + } + } + else + { + // all fixed bodies (inf mass) get mapped to a single solver id + if ( m_fixedBodyId < 0 ) + { + m_bodySolverArrayMutex.lock(); + // now that we have the lock, check again + if ( m_fixedBodyId < 0 ) + { + m_fixedBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody( &fixedBody, 0, timeStep ); + } + m_bodySolverArrayMutex.unlock(); + } + solverBodyId = m_fixedBodyId; + } + btAssert( solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size() ); + return solverBodyId; +} + + +void btSequentialImpulseConstraintSolverMt::internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("internalCollectContactManifoldCachedInfo"); + for (int i = 0; i < numManifolds; ++i) + { + btContactManifoldCachedInfo* cachedInfo = &cachedInfoArray[i]; + btPersistentManifold* manifold = manifoldPtr[i]; + btCollisionObject* colObj0 = (btCollisionObject*) manifold->getBody0(); + btCollisionObject* colObj1 = (btCollisionObject*) manifold->getBody1(); + + int solverBodyIdA = getOrInitSolverBodyThreadsafe( *colObj0, infoGlobal.m_timeStep ); + int solverBodyIdB = getOrInitSolverBodyThreadsafe( *colObj1, infoGlobal.m_timeStep ); + + cachedInfo->solverBodyIds[ 0 ] = solverBodyIdA; + cachedInfo->solverBodyIds[ 1 ] = solverBodyIdB; + cachedInfo->numTouchingContacts = 0; + + btSolverBody* solverBodyA = &m_tmpSolverBodyPool[ solverBodyIdA ]; + btSolverBody* solverBodyB = &m_tmpSolverBodyPool[ solverBodyIdB ]; + + // A contact manifold between 2 static object should not exist! + // check the collision flags of your objects if this assert fires. + // Incorrectly set collision object flags can degrade performance in various ways. + btAssert( !m_tmpSolverBodyPool[ solverBodyIdA ].m_invMass.isZero() || !m_tmpSolverBodyPool[ solverBodyIdB ].m_invMass.isZero() ); + + int iContact = 0; + for ( int j = 0; j < manifold->getNumContacts(); j++ ) + { + btManifoldPoint& cp = manifold->getContactPoint( j ); + + if ( cp.getDistance() <= manifold->getContactProcessingThreshold() ) + { + cachedInfo->contactPoints[ iContact ] = &cp; + cachedInfo->contactHasRollingFriction[ iContact ] = ( cp.m_combinedRollingFriction > 0.f ); + iContact++; + } + } + cachedInfo->numTouchingContacts = iContact; + } +} + + +struct CollectContactManifoldCachedInfoLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray; + btPersistentManifold** m_manifoldPtr; + const btContactSolverInfo* m_infoGlobal; + + CollectContactManifoldCachedInfoLoop( btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, const btContactSolverInfo& infoGlobal ) + { + m_solver = solver; + m_cachedInfoArray = cachedInfoArray; + m_manifoldPtr = manifoldPtr; + m_infoGlobal = &infoGlobal; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalCollectContactManifoldCachedInfo( m_cachedInfoArray + iBegin, m_manifoldPtr + iBegin, iEnd - iBegin, *m_infoGlobal ); + } +}; + + +void btSequentialImpulseConstraintSolverMt::internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds) +{ + BT_PROFILE("internalAllocContactConstraints"); + // possibly parallel part + for ( int iManifold = 0; iManifold < numManifolds; ++iManifold ) + { + const btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[ iManifold ]; + int contactIndex = cachedInfo.contactIndex; + int frictionIndex = contactIndex * m_numFrictionDirections; + int rollingFrictionIndex = cachedInfo.rollingFrictionIndex; + for ( int i = 0; i < cachedInfo.numTouchingContacts; i++ ) + { + btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[contactIndex]; + contactConstraint.m_solverBodyIdA = cachedInfo.solverBodyIds[ 0 ]; + contactConstraint.m_solverBodyIdB = cachedInfo.solverBodyIds[ 1 ]; + contactConstraint.m_originalContactPoint = cachedInfo.contactPoints[ i ]; + + // allocate the friction constraints + contactConstraint.m_frictionIndex = frictionIndex; + for ( int iDir = 0; iDir < m_numFrictionDirections; ++iDir ) + { + btSolverConstraint& frictionConstraint = m_tmpSolverContactFrictionConstraintPool[frictionIndex]; + frictionConstraint.m_frictionIndex = contactIndex; + frictionIndex++; + } + + // allocate rolling friction constraints + if ( cachedInfo.contactHasRollingFriction[ i ] ) + { + m_rollingFrictionIndexTable[ contactIndex ] = rollingFrictionIndex; + // allocate 3 (although we may use only 2 sometimes) + for ( int i = 0; i < 3; i++ ) + { + m_tmpSolverContactRollingFrictionConstraintPool[ rollingFrictionIndex ].m_frictionIndex = contactIndex; + rollingFrictionIndex++; + } + } + else + { + // indicate there is no rolling friction for this contact point + m_rollingFrictionIndexTable[ contactIndex ] = -1; + } + contactIndex++; + } + } +} + + +struct AllocContactConstraintsLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray; + + AllocContactConstraintsLoop( btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray ) + { + m_solver = solver; + m_cachedInfoArray = cachedInfoArray; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalAllocContactConstraints( m_cachedInfoArray + iBegin, iEnd - iBegin ); + } +}; + + +void btSequentialImpulseConstraintSolverMt::allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE( "allocAllContactConstraints" ); + btAlignedObjectArray cachedInfoArray; // = m_manifoldCachedInfoArray; + cachedInfoArray.resizeNoInitialize( numManifolds ); + if (/* DISABLES CODE */ (false)) + { + // sequential + internalCollectContactManifoldCachedInfo(&cachedInfoArray[ 0 ], manifoldPtr, numManifolds, infoGlobal); + } + else + { + // may alter ordering of bodies which affects determinism + CollectContactManifoldCachedInfoLoop loop( this, &cachedInfoArray[ 0 ], manifoldPtr, infoGlobal ); + int grainSize = 200; + btParallelFor( 0, numManifolds, grainSize, loop ); + } + + { + // serial part + int numContacts = 0; + int numRollingFrictionConstraints = 0; + for ( int iManifold = 0; iManifold < numManifolds; ++iManifold ) + { + btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[ iManifold ]; + cachedInfo.contactIndex = numContacts; + cachedInfo.rollingFrictionIndex = numRollingFrictionConstraints; + numContacts += cachedInfo.numTouchingContacts; + for (int i = 0; i < cachedInfo.numTouchingContacts; ++i) + { + if (cachedInfo.contactHasRollingFriction[i]) + { + numRollingFrictionConstraints += 3; + } + } + } + { + BT_PROFILE( "allocPools" ); + if ( m_tmpSolverContactConstraintPool.capacity() < numContacts ) + { + // if we need to reallocate, reserve some extra so we don't have to reallocate again next frame + int extraReserve = numContacts / 16; + m_tmpSolverContactConstraintPool.reserve( numContacts + extraReserve ); + m_rollingFrictionIndexTable.reserve( numContacts + extraReserve ); + m_tmpSolverContactFrictionConstraintPool.reserve( ( numContacts + extraReserve )*m_numFrictionDirections ); + m_tmpSolverContactRollingFrictionConstraintPool.reserve( numRollingFrictionConstraints + extraReserve ); + } + m_tmpSolverContactConstraintPool.resizeNoInitialize( numContacts ); + m_rollingFrictionIndexTable.resizeNoInitialize( numContacts ); + m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize( numContacts*m_numFrictionDirections ); + m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize( numRollingFrictionConstraints ); + } + } + { + AllocContactConstraintsLoop loop(this, &cachedInfoArray[0]); + int grainSize = 200; + btParallelFor( 0, numManifolds, grainSize, loop ); + } +} + + +void btSequentialImpulseConstraintSolverMt::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) +{ + if (!m_useBatching) + { + btSequentialImpulseConstraintSolver::convertContacts(manifoldPtr, numManifolds, infoGlobal); + return; + } + BT_PROFILE( "convertContacts" ); + if (numManifolds > 0) + { + if ( m_fixedBodyId < 0 ) + { + m_fixedBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody( &fixedBody, 0, infoGlobal.m_timeStep ); + } + allocAllContactConstraints( manifoldPtr, numManifolds, infoGlobal ); + if ( m_useBatching ) + { + setupBatchedContactConstraints(); + } + setupAllContactConstraints( infoGlobal ); + } +} + + +void btSequentialImpulseConstraintSolverMt::internalInitMultipleJoints( btTypedConstraint** constraints, int iBegin, int iEnd ) +{ + BT_PROFILE("internalInitMultipleJoints"); + for ( int i = iBegin; i < iEnd; i++ ) + { + btTypedConstraint* constraint = constraints[i]; + btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + if (constraint->isEnabled()) + { + constraint->buildJacobian(); + constraint->internalSetAppliedImpulse( 0.0f ); + btJointFeedback* fb = constraint->getJointFeedback(); + if ( fb ) + { + fb->m_appliedForceBodyA.setZero(); + fb->m_appliedTorqueBodyA.setZero(); + fb->m_appliedForceBodyB.setZero(); + fb->m_appliedTorqueBodyB.setZero(); + } + constraint->getInfo1( &info1 ); + } + else + { + info1.m_numConstraintRows = 0; + info1.nub = 0; + } + } +} + + +struct InitJointsLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + btTypedConstraint** m_constraints; + + InitJointsLoop( btSequentialImpulseConstraintSolverMt* solver, btTypedConstraint** constraints ) + { + m_solver = solver; + m_constraints = constraints; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalInitMultipleJoints( m_constraints, iBegin, iEnd ); + } +}; + + +void btSequentialImpulseConstraintSolverMt::internalConvertMultipleJoints( const btAlignedObjectArray& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ) +{ + BT_PROFILE("internalConvertMultipleJoints"); + for ( int i = iBegin; i < iEnd; ++i ) + { + const JointParams& jointParams = jointParamsArray[ i ]; + int currentRow = jointParams.m_solverConstraint; + if ( currentRow != -1 ) + { + const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[ i ]; + btAssert( currentRow < m_tmpSolverNonContactConstraintPool.size() ); + btAssert( info1.m_numConstraintRows > 0 ); + + btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[ currentRow ]; + btTypedConstraint* constraint = constraints[ i ]; + + convertJoint( currentConstraintRow, constraint, info1, jointParams.m_solverBodyA, jointParams.m_solverBodyB, infoGlobal ); + } + } +} + + +struct ConvertJointsLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btAlignedObjectArray& m_jointParamsArray; + btTypedConstraint** m_srcConstraints; + const btContactSolverInfo& m_infoGlobal; + + ConvertJointsLoop( btSequentialImpulseConstraintSolverMt* solver, + const btAlignedObjectArray& jointParamsArray, + btTypedConstraint** srcConstraints, + const btContactSolverInfo& infoGlobal + ) : + m_jointParamsArray(jointParamsArray), + m_infoGlobal(infoGlobal) + { + m_solver = solver; + m_srcConstraints = srcConstraints; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalConvertMultipleJoints( m_jointParamsArray, m_srcConstraints, iBegin, iEnd, m_infoGlobal ); + } +}; + + +void btSequentialImpulseConstraintSolverMt::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) +{ + if ( !m_useBatching ) + { + btSequentialImpulseConstraintSolver::convertJoints(constraints, numConstraints, infoGlobal); + return; + } + BT_PROFILE("convertJoints"); + bool parallelJointSetup = true; + m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); + if (parallelJointSetup) + { + InitJointsLoop loop(this, constraints); + int grainSize = 40; + btParallelFor(0, numConstraints, grainSize, loop); + } + else + { + internalInitMultipleJoints( constraints, 0, numConstraints ); + } + + int totalNumRows = 0; + btAlignedObjectArray jointParamsArray; + jointParamsArray.resizeNoInitialize(numConstraints); + + //calculate the total number of contraint rows + for (int i=0;igetRigidBodyA(), infoGlobal.m_timeStep ); + params.m_solverBodyB = getOrInitSolverBody( constraint->getRigidBodyB(), infoGlobal.m_timeStep ); + } + else + { + params.m_solverConstraint = -1; + } + totalNumRows += info1.m_numConstraintRows; + } + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); + + ///setup the btSolverConstraints + if ( parallelJointSetup ) + { + ConvertJointsLoop loop(this, jointParamsArray, constraints, infoGlobal); + int grainSize = 20; + btParallelFor(0, numConstraints, grainSize, loop); + } + else + { + internalConvertMultipleJoints( jointParamsArray, constraints, 0, numConstraints, infoGlobal ); + } + setupBatchedJointConstraints(); +} + + +void btSequentialImpulseConstraintSolverMt::internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("internalConvertBodies"); + for (int i=iBegin; i < iEnd; i++) + { + btCollisionObject* obj = bodies[i]; + obj->setCompanionId(i); + btSolverBody& solverBody = m_tmpSolverBodyPool[i]; + initSolverBody(&solverBody, obj, infoGlobal.m_timeStep); + + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getInvMass()) + { + btVector3 gyroForce (0,0,0); + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) + { + gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce); + solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; + } + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD) + { + gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep); + solverBody.m_externalTorqueImpulse += gyroForce; + } + if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY) + { + gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep); + solverBody.m_externalTorqueImpulse += gyroForce; + } + } + } +} + + +struct ConvertBodiesLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + btCollisionObject** m_bodies; + int m_numBodies; + const btContactSolverInfo& m_infoGlobal; + + ConvertBodiesLoop( btSequentialImpulseConstraintSolverMt* solver, + btCollisionObject** bodies, + int numBodies, + const btContactSolverInfo& infoGlobal + ) : + m_infoGlobal(infoGlobal) + { + m_solver = solver; + m_bodies = bodies; + m_numBodies = numBodies; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalConvertBodies( m_bodies, iBegin, iEnd, m_infoGlobal ); + } +}; + + +void btSequentialImpulseConstraintSolverMt::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("convertBodies"); + m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 ); + + m_tmpSolverBodyPool.resizeNoInitialize(numBodies+1); + + m_fixedBodyId = numBodies; + { + btSolverBody& fixedBody = m_tmpSolverBodyPool[ m_fixedBodyId ]; + initSolverBody( &fixedBody, NULL, infoGlobal.m_timeStep ); + } + + bool parallelBodySetup = true; + if (parallelBodySetup) + { + ConvertBodiesLoop loop(this, bodies, numBodies, infoGlobal); + int grainSize = 40; + btParallelFor(0, numBodies, grainSize, loop); + } + else + { + internalConvertBodies( bodies, 0, numBodies, infoGlobal ); + } +} + + +btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySetup( + btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifoldPtr, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& infoGlobal, + btIDebugDraw* debugDrawer + ) +{ + m_numFrictionDirections = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1; + m_useBatching = false; + if ( numManifolds >= s_minimumContactManifoldsForBatching && + (s_allowNestedParallelForLoops || !btThreadsAreRunning()) + ) + { + m_useBatching = true; + m_batchedContactConstraints.m_debugDrawer = debugDrawer; + m_batchedJointConstraints.m_debugDrawer = debugDrawer; + } + btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies, + numBodies, + manifoldPtr, + numManifolds, + constraints, + numConstraints, + infoGlobal, + debugDrawer + ); + return 0.0f; +} + + +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd ) +{ + btScalar leastSquaresResidual = 0.f; + for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) + { + int iCons = consIndices[ iiCons ]; + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iCons ]; + btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ]; + btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ]; + btScalar residual = resolveSplitPenetrationImpulse( bodyA, bodyB, solveManifold ); + leastSquaresResidual += residual*residual; + } + return leastSquaresResidual; +} + + +struct ContactSplitPenetrationImpulseSolverLoop : public btIParallelSumBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + + ContactSplitPenetrationImpulseSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "ContactSplitPenetrationImpulseSolverLoop" ); + btScalar sum = 0; + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + sum += m_solver->resolveMultipleContactSplitPenetrationImpulseConstraints( m_bc->m_constraintIndices, batch.begin, batch.end ); + } + return sum; + } +}; + + +void btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); + if (infoGlobal.m_splitImpulse) + { + for ( int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++ ) + { + btScalar leastSquaresResidual = 0.f; + if (m_useBatching) + { + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + ContactSplitPenetrationImpulseSolverLoop loop( this, &batchedCons ); + btScalar leastSquaresResidual = 0.f; + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = batchedCons.m_phaseGrainSize[iPhase]; + leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); + } + } + else + { + // non-batched + leastSquaresResidual = resolveMultipleContactSplitPenetrationImpulseConstraints(m_orderTmpConstraintPool, 0, m_tmpSolverContactConstraintPool.size()); + } + if ( leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= ( infoGlobal.m_numIterations - 1 ) ) + { +#ifdef VERBOSE_RESIDUAL_PRINTF + printf( "residual = %f at iteration #%d\n", leastSquaresResidual, iteration ); +#endif + break; + } + } + } +} + + +btScalar btSequentialImpulseConstraintSolverMt::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + if ( !m_useBatching ) + { + return btSequentialImpulseConstraintSolver::solveSingleIteration( iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer ); + } + BT_PROFILE( "solveSingleIterationMt" ); + btScalar leastSquaresResidual = 0.f; + + if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) + { + if (1) // uncomment this for a bit less random ((iteration & 7) == 0) + { + randomizeConstraintOrdering(iteration, infoGlobal.m_numIterations); + } + } + + { + ///solve all joint constraints + leastSquaresResidual += resolveAllJointConstraints(iteration); + + if (iteration< infoGlobal.m_numIterations) + { + // this loop is only used for cone-twist constraints, + // it would be nice to skip this loop if none of the constraints need it + if ( m_useObsoleteJointConstraints ) + { + for ( int j = 0; jisEnabled() ) + { + int bodyAid = getOrInitSolverBody( constraints[ j ]->getRigidBodyA(), infoGlobal.m_timeStep ); + int bodyBid = getOrInitSolverBody( constraints[ j ]->getRigidBodyB(), infoGlobal.m_timeStep ); + btSolverBody& bodyA = m_tmpSolverBodyPool[ bodyAid ]; + btSolverBody& bodyB = m_tmpSolverBodyPool[ bodyBid ]; + constraints[ j ]->solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep ); + } + } + } + + if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) + { + // solve all contact, contact-friction, and rolling friction constraints interleaved + leastSquaresResidual += resolveAllContactConstraintsInterleaved(); + } + else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS + { + // don't interleave them + // solve all contact constraints + leastSquaresResidual += resolveAllContactConstraints(); + + // solve all contact friction constraints + leastSquaresResidual += resolveAllContactFrictionConstraints(); + + // solve all rolling friction constraints + leastSquaresResidual += resolveAllRollingFrictionConstraints(); + } + } + } + return leastSquaresResidual; +} + + +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleJointConstraints( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd, int iteration ) +{ + btScalar leastSquaresResidual = 0.f; + for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) + { + int iCons = consIndices[ iiCons ]; + const btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[ iCons ]; + if ( iteration < constraint.m_overrideNumSolverIterations ) + { + btSolverBody& bodyA = m_tmpSolverBodyPool[ constraint.m_solverBodyIdA ]; + btSolverBody& bodyB = m_tmpSolverBodyPool[ constraint.m_solverBodyIdB ]; + btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, constraint ); + leastSquaresResidual += residual*residual; + } + } + return leastSquaresResidual; +} + + +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraints( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd ) +{ + btScalar leastSquaresResidual = 0.f; + for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) + { + int iCons = consIndices[ iiCons ]; + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iCons ]; + btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ]; + btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ]; + btScalar residual = resolveSingleConstraintRowLowerLimit( bodyA, bodyB, solveManifold ); + leastSquaresResidual += residual*residual; + } + return leastSquaresResidual; +} + + +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactFrictionConstraints( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd ) +{ + btScalar leastSquaresResidual = 0.f; + for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) + { + int iContact = consIndices[ iiCons ]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[ iContact ].m_appliedImpulse; + + // apply sliding friction + if ( totalImpulse > 0.0f ) + { + int iBegin = iContact * m_numFrictionDirections; + int iEnd = iBegin + m_numFrictionDirections; + for ( int iFriction = iBegin; iFriction < iEnd; ++iFriction ) + { + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[ iFriction++ ]; + btAssert( solveManifold.m_frictionIndex == iContact ); + + solveManifold.m_lowerLimit = -( solveManifold.m_friction*totalImpulse ); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ]; + btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ]; + btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, solveManifold ); + leastSquaresResidual += residual*residual; + } + } + } + return leastSquaresResidual; +} + + +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd ) +{ + btScalar leastSquaresResidual = 0.f; + for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) + { + int iContact = consIndices[ iiCons ]; + int iFirstRollingFriction = m_rollingFrictionIndexTable[ iContact ]; + if ( iFirstRollingFriction >= 0 ) + { + btScalar totalImpulse = m_tmpSolverContactConstraintPool[ iContact ].m_appliedImpulse; + // apply rolling friction + if ( totalImpulse > 0.0f ) + { + int iBegin = iFirstRollingFriction; + int iEnd = iBegin + 3; + for ( int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric ) + { + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ]; + if ( rollingFrictionConstraint.m_frictionIndex != iContact ) + { + break; + } + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction ) + { + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + } + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint ); + leastSquaresResidual += residual*residual; + } + } + } + } + return leastSquaresResidual; +} + + +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray& contactIndices, + int batchBegin, + int batchEnd + ) +{ + btScalar leastSquaresResidual = 0.f; + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + + for ( int iiCons = batchBegin; iiCons < batchEnd; iiCons++ ) + { + btScalar totalImpulse = 0; + int iContact = contactIndices[ iiCons ]; + // apply penetration constraint + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iContact ]; + btScalar residual = resolveSingleConstraintRowLowerLimit( m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ], m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ], solveManifold ); + leastSquaresResidual += residual*residual; + totalImpulse = solveManifold.m_appliedImpulse; + } + + // apply sliding friction + if ( totalImpulse > 0.0f ) + { + int iBegin = iContact * m_numFrictionDirections; + int iEnd = iBegin + m_numFrictionDirections; + for ( int iFriction = iBegin; iFriction < iEnd; ++iFriction ) + { + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[ iFriction ]; + btAssert( solveManifold.m_frictionIndex == iContact ); + + solveManifold.m_lowerLimit = -( solveManifold.m_friction*totalImpulse ); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ]; + btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ]; + btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, solveManifold ); + leastSquaresResidual += residual*residual; + } + } + + // apply rolling friction + int iFirstRollingFriction = m_rollingFrictionIndexTable[ iContact ]; + if ( totalImpulse > 0.0f && iFirstRollingFriction >= 0) + { + int iBegin = iFirstRollingFriction; + int iEnd = iBegin + 3; + for ( int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric ) + { + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ]; + if ( rollingFrictionConstraint.m_frictionIndex != iContact ) + { + break; + } + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction ) + { + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + } + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint ); + leastSquaresResidual += residual*residual; + } + } + } + return leastSquaresResidual; +} + + +void btSequentialImpulseConstraintSolverMt::randomizeBatchedConstraintOrdering( btBatchedConstraints* batchedConstraints ) +{ + btBatchedConstraints& bc = *batchedConstraints; + // randomize ordering of phases + for ( int ii = 1; ii < bc.m_phaseOrder.size(); ++ii ) + { + int iSwap = btRandInt2( ii + 1 ); + bc.m_phaseOrder.swap( ii, iSwap ); + } + + // for each batch, + for ( int iBatch = 0; iBatch < bc.m_batches.size(); ++iBatch ) + { + // randomize ordering of constraints within the batch + const btBatchedConstraints::Range& batch = bc.m_batches[ iBatch ]; + for ( int iiCons = batch.begin; iiCons < batch.end; ++iiCons ) + { + int iSwap = batch.begin + btRandInt2( iiCons - batch.begin + 1 ); + btAssert(iSwap >= batch.begin && iSwap < batch.end); + bc.m_constraintIndices.swap( iiCons, iSwap ); + } + } +} + + +void btSequentialImpulseConstraintSolverMt::randomizeConstraintOrdering(int iteration, int numIterations) +{ + // randomize ordering of joint constraints + randomizeBatchedConstraintOrdering( &m_batchedJointConstraints ); + + //contact/friction constraints are not solved more than numIterations + if ( iteration < numIterations ) + { + randomizeBatchedConstraintOrdering( &m_batchedContactConstraints ); + } +} + + +struct JointSolverLoop : public btIParallelSumBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + int m_iteration; + + JointSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, int iteration ) + { + m_solver = solver; + m_bc = bc; + m_iteration = iteration; + } + btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "JointSolverLoop" ); + btScalar sum = 0; + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + sum += m_solver->resolveMultipleJointConstraints( m_bc->m_constraintIndices, batch.begin, batch.end, m_iteration ); + } + return sum; + } +}; + + +btScalar btSequentialImpulseConstraintSolverMt::resolveAllJointConstraints(int iteration) +{ + BT_PROFILE( "resolveAllJointConstraints" ); + const btBatchedConstraints& batchedCons = m_batchedJointConstraints; + JointSolverLoop loop( this, &batchedCons, iteration ); + btScalar leastSquaresResidual = 0.f; + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = 1; + leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); + } + return leastSquaresResidual; +} + + +struct ContactSolverLoop : public btIParallelSumBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + + ContactSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "ContactSolverLoop" ); + btScalar sum = 0; + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + sum += m_solver->resolveMultipleContactConstraints( m_bc->m_constraintIndices, batch.begin, batch.end ); + } + return sum; + } +}; + + +btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraints() +{ + BT_PROFILE( "resolveAllContactConstraints" ); + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + ContactSolverLoop loop( this, &batchedCons ); + btScalar leastSquaresResidual = 0.f; + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = batchedCons.m_phaseGrainSize[iPhase]; + leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); + } + return leastSquaresResidual; +} + + +struct ContactFrictionSolverLoop : public btIParallelSumBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + + ContactFrictionSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "ContactFrictionSolverLoop" ); + btScalar sum = 0; + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + sum += m_solver->resolveMultipleContactFrictionConstraints( m_bc->m_constraintIndices, batch.begin, batch.end ); + } + return sum; + } +}; + + +btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactFrictionConstraints() +{ + BT_PROFILE( "resolveAllContactFrictionConstraints" ); + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + ContactFrictionSolverLoop loop( this, &batchedCons ); + btScalar leastSquaresResidual = 0.f; + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = batchedCons.m_phaseGrainSize[iPhase]; + leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); + } + return leastSquaresResidual; +} + + +struct InterleavedContactSolverLoop : public btIParallelSumBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + + InterleavedContactSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "InterleavedContactSolverLoop" ); + btScalar sum = 0; + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + sum += m_solver->resolveMultipleContactConstraintsInterleaved( m_bc->m_constraintIndices, batch.begin, batch.end ); + } + return sum; + } +}; + + +btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraintsInterleaved() +{ + BT_PROFILE( "resolveAllContactConstraintsInterleaved" ); + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + InterleavedContactSolverLoop loop( this, &batchedCons ); + btScalar leastSquaresResidual = 0.f; + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = 1; + leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); + } + return leastSquaresResidual; +} + + +struct ContactRollingFrictionSolverLoop : public btIParallelSumBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + + ContactRollingFrictionSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + BT_PROFILE( "ContactFrictionSolverLoop" ); + btScalar sum = 0; + for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; + sum += m_solver->resolveMultipleContactRollingFrictionConstraints( m_bc->m_constraintIndices, batch.begin, batch.end ); + } + return sum; + } +}; + + +btScalar btSequentialImpulseConstraintSolverMt::resolveAllRollingFrictionConstraints() +{ + BT_PROFILE( "resolveAllRollingFrictionConstraints" ); + btScalar leastSquaresResidual = 0.f; + // + // We do not generate batches for rolling friction constraints. We assume that + // one of two cases is true: + // + // 1. either most bodies in the simulation have rolling friction, in which case we can use the + // batches for contacts and use a lookup table to translate contact indices to rolling friction + // (ignoring any contact indices that don't map to a rolling friction constraint). As long as + // most contacts have a corresponding rolling friction constraint, this should parallelize well. + // + // -OR- + // + // 2. few bodies in the simulation have rolling friction, so it is not worth trying to use the + // batches from contacts as most of the contacts won't have corresponding rolling friction + // constraints and most threads would end up doing very little work. Most of the time would + // go to threading overhead, so we don't bother with threading. + // + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + if (numRollingFrictionPoolConstraints >= m_tmpSolverContactConstraintPool.size()) + { + // use batching if there are many rolling friction constraints + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + ContactRollingFrictionSolverLoop loop( this, &batchedCons ); + btScalar leastSquaresResidual = 0.f; + for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) + { + int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; + int grainSize = 1; + leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); + } + } + else + { + // no batching, also ignores SOLVER_RANDMIZE_ORDER + for ( int j = 0; j < numRollingFrictionPoolConstraints; j++ ) + { + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ j ]; + if ( rollingFrictionConstraint.m_frictionIndex >= 0 ) + { + btScalar totalImpulse = m_tmpSolverContactConstraintPool[ rollingFrictionConstraint.m_frictionIndex ].m_appliedImpulse; + if ( totalImpulse > 0.0f ) + { + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction ) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint ); + leastSquaresResidual += residual*residual; + } + } + } + } + return leastSquaresResidual; +} + + +void btSequentialImpulseConstraintSolverMt::internalWriteBackContacts( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ) +{ + BT_PROFILE("internalWriteBackContacts"); + writeBackContacts(iBegin, iEnd, infoGlobal); + //for ( int iContact = iBegin; iContact < iEnd; ++iContact) + //{ + // const btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[ iContact ]; + // btManifoldPoint* pt = (btManifoldPoint*) contactConstraint.m_originalContactPoint; + // btAssert( pt ); + // pt->m_appliedImpulse = contactConstraint.m_appliedImpulse; + // pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex ].m_appliedImpulse; + // if ( m_numFrictionDirections == 2 ) + // { + // pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex + 1 ].m_appliedImpulse; + // } + //} +} + + +void btSequentialImpulseConstraintSolverMt::internalWriteBackJoints( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ) +{ + BT_PROFILE("internalWriteBackJoints"); + writeBackJoints(iBegin, iEnd, infoGlobal); +} + + +void btSequentialImpulseConstraintSolverMt::internalWriteBackBodies( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ) +{ + BT_PROFILE("internalWriteBackBodies"); + writeBackBodies( iBegin, iEnd, infoGlobal ); +} + + +struct WriteContactPointsLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btContactSolverInfo* m_infoGlobal; + + WriteContactPointsLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal ) + { + m_solver = solver; + m_infoGlobal = &infoGlobal; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalWriteBackContacts( iBegin, iEnd, *m_infoGlobal ); + } +}; + + +struct WriteJointsLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btContactSolverInfo* m_infoGlobal; + + WriteJointsLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal ) + { + m_solver = solver; + m_infoGlobal = &infoGlobal; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalWriteBackJoints( iBegin, iEnd, *m_infoGlobal ); + } +}; + + +struct WriteBodiesLoop : public btIParallelForBody +{ + btSequentialImpulseConstraintSolverMt* m_solver; + const btContactSolverInfo* m_infoGlobal; + + WriteBodiesLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal ) + { + m_solver = solver; + m_infoGlobal = &infoGlobal; + } + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + m_solver->internalWriteBackBodies( iBegin, iEnd, *m_infoGlobal ); + } +}; + + +btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) +{ + BT_PROFILE("solveGroupCacheFriendlyFinish"); + + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + WriteContactPointsLoop loop( this, infoGlobal ); + int grainSize = 500; + btParallelFor( 0, m_tmpSolverContactConstraintPool.size(), grainSize, loop ); + } + + { + WriteJointsLoop loop( this, infoGlobal ); + int grainSize = 400; + btParallelFor( 0, m_tmpSolverNonContactConstraintPool.size(), grainSize, loop ); + } + { + WriteBodiesLoop loop( this, infoGlobal ); + int grainSize = 100; + btParallelFor( 0, m_tmpSolverBodyPool.size(), grainSize, loop ); + } + + m_tmpSolverContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); + + m_tmpSolverBodyPool.resizeNoInitialize(0); + return 0.f; +} + diff --git a/extern/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h new file mode 100644 index 000000000000..55d53474c473 --- /dev/null +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h @@ -0,0 +1,154 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H +#define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H + +#include "btSequentialImpulseConstraintSolver.h" +#include "btBatchedConstraints.h" +#include "LinearMath/btThreads.h" + +/// +/// btSequentialImpulseConstraintSolverMt +/// +/// A multithreaded variant of the sequential impulse constraint solver. The constraints to be solved are grouped into +/// batches and phases where each batch of constraints within a given phase can be solved in parallel with the rest. +/// Ideally we want as few phases as possible, and each phase should have many batches, and all of the batches should +/// have about the same number of constraints. +/// This method works best on a large island of many constraints. +/// +/// Supports all of the features of the normal sequential impulse solver such as: +/// - split penetration impulse +/// - rolling friction +/// - interleaving constraints +/// - warmstarting +/// - 2 friction directions +/// - randomized constraint ordering +/// - early termination when leastSquaresResidualThreshold is satisfied +/// +/// When the SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS flag is enabled, unlike the normal SequentialImpulse solver, +/// the rolling friction is interleaved as well. +/// Interleaving the contact penetration constraints with friction reduces the number of parallel loops that need to be done, +/// which reduces threading overhead so it can be a performance win, however, it does seem to produce a less stable simulation, +/// at least on stacks of blocks. +/// +/// When the SOLVER_RANDMIZE_ORDER flag is enabled, the ordering of phases, and the ordering of constraints within each batch +/// is randomized, however it does not swap constraints between batches. +/// This is to avoid regenerating the batches for each solver iteration which would be quite costly in performance. +/// +/// Note that a non-zero leastSquaresResidualThreshold could possibly affect the determinism of the simulation +/// if the task scheduler's parallelSum operation is non-deterministic. The parallelSum operation can be non-deterministic +/// because floating point addition is not associative due to rounding errors. +/// The task scheduler can and should ensure that the result of any parallelSum operation is deterministic. +/// +ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolver +{ +public: + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE; + virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE; + virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) BT_OVERRIDE; + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE; + + // temp struct used to collect info from persistent manifolds into a cache-friendly struct using multiple threads + struct btContactManifoldCachedInfo + { + static const int MAX_NUM_CONTACT_POINTS = 4; + + int numTouchingContacts; + int solverBodyIds[ 2 ]; + int contactIndex; + int rollingFrictionIndex; + bool contactHasRollingFriction[ MAX_NUM_CONTACT_POINTS ]; + btManifoldPoint* contactPoints[ MAX_NUM_CONTACT_POINTS ]; + }; + // temp struct used for setting up joint constraints in parallel + struct JointParams + { + int m_solverConstraint; + int m_solverBodyA; + int m_solverBodyB; + }; + void internalInitMultipleJoints(btTypedConstraint** constraints, int iBegin, int iEnd); + void internalConvertMultipleJoints( const btAlignedObjectArray& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ); + + // parameters to control batching + static bool s_allowNestedParallelForLoops; // whether to allow nested parallel operations + static int s_minimumContactManifoldsForBatching; // don't even try to batch if fewer manifolds than this + static btBatchedConstraints::BatchingMethod s_contactBatchingMethod; + static btBatchedConstraints::BatchingMethod s_jointBatchingMethod; + static int s_minBatchSize; // desired number of constraints per batch + static int s_maxBatchSize; + +protected: + static const int CACHE_LINE_SIZE = 64; + + btBatchedConstraints m_batchedContactConstraints; + btBatchedConstraints m_batchedJointConstraints; + int m_numFrictionDirections; + bool m_useBatching; + bool m_useObsoleteJointConstraints; + btAlignedObjectArray m_manifoldCachedInfoArray; + btAlignedObjectArray m_rollingFrictionIndexTable; // lookup table mapping contact index to rolling friction index + btSpinMutex m_bodySolverArrayMutex; + char m_antiFalseSharingPadding[CACHE_LINE_SIZE]; // padding to keep mutexes in separate cachelines + btSpinMutex m_kinematicBodyUniqueIdToSolverBodyTableMutex; + btAlignedObjectArray m_scratchMemory; + + virtual void randomizeConstraintOrdering( int iteration, int numIterations ); + virtual btScalar resolveAllJointConstraints( int iteration ); + virtual btScalar resolveAllContactConstraints(); + virtual btScalar resolveAllContactFrictionConstraints(); + virtual btScalar resolveAllContactConstraintsInterleaved(); + virtual btScalar resolveAllRollingFrictionConstraints(); + + virtual void setupBatchedContactConstraints(); + virtual void setupBatchedJointConstraints(); + virtual void convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal) BT_OVERRIDE; + virtual void convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) BT_OVERRIDE; + virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE; + + int getOrInitSolverBodyThreadsafe(btCollisionObject& body, btScalar timeStep); + void allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); + void setupAllContactConstraints(const btContactSolverInfo& infoGlobal); + void randomizeBatchedConstraintOrdering( btBatchedConstraints* batchedConstraints ); + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btSequentialImpulseConstraintSolverMt(); + virtual ~btSequentialImpulseConstraintSolverMt(); + + btScalar resolveMultipleJointConstraints( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd, int iteration ); + btScalar resolveMultipleContactConstraints( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd ); + btScalar resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd ); + btScalar resolveMultipleContactFrictionConstraints( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd ); + btScalar resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray& consIndices, int batchBegin, int batchEnd ); + btScalar resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray& contactIndices, int batchBegin, int batchEnd ); + + void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal); + void internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds); + void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal); + void internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); + void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal); +}; + + + + +#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_MT_H + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp old mode 100644 new mode 100755 similarity index 99% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index f8f81bfe6fa5..d63cef031626 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -364,7 +364,6 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra int srow; btScalar limit_err; int limit; - int powered; // next two rows. // we want: velA + wA x relA == velB + wB x relB ... but this would @@ -470,13 +469,9 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra limit_err = getLinDepth() * signFact; limit = (limit_err > btScalar(0.0)) ? 2 : 1; } - powered = 0; - if(getPoweredLinMotor()) - { - powered = 1; - } + bool powered = getPoweredLinMotor(); // if the slider has joint limits or motor, add in the extra row - if (limit || powered) + if (limit || powered) { nrow++; srow = nrow * info->rowskip; @@ -524,7 +519,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra btScalar histop = getUpperLinLimit(); if(limit && (lostop == histop)) { // the joint motor is ineffective - powered = 0; + powered = false; } info->m_constraintError[srow] = 0.; info->m_lowerLimit[srow] = 0.; @@ -609,12 +604,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra limit = (limit_err > btScalar(0.0)) ? 1 : 2; } // if the slider has joint limits, add in the extra row - powered = 0; - if(getPoweredAngMotor()) - { - powered = 1; - } - if(limit || powered) + powered = getPoweredAngMotor(); + if(limit || powered) { nrow++; srow = nrow * info->rowskip; @@ -630,7 +621,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra btScalar histop = getUpperAngLimit(); if(limit && (lostop == histop)) { // the joint motor is ineffective - powered = 0; + powered = false; } currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp; if(powered) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h old mode 100644 new mode 100755 similarity index 99% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 628ada4cfb11..1957f08a96c8 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -25,6 +25,8 @@ April 04, 2008 #ifndef BT_SLIDER_CONSTRAINT_H #define BT_SLIDER_CONSTRAINT_H +#include "LinearMath/btScalar.h"//for BT_USE_DOUBLE_PRECISION + #ifdef BT_USE_DOUBLE_PRECISION #define btSliderConstraintData2 btSliderConstraintDoubleData #define btSliderConstraintDataName "btSliderConstraintDoubleData" diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btSolverBody.h diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp similarity index 99% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 736a64a1c943..9f04f2805395 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -19,7 +19,7 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" -#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f) +#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f) btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) :btTypedObject(type), diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h similarity index 98% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index b55db0c1bfb4..8a2a2d1ae7e7 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -145,11 +145,6 @@ ATTRIBUTE_ALIGNED16(class) btTypedConstraint : public btTypedObject // lo and hi limits for variables (set to -/+ infinity on entry). btScalar *m_lowerLimit,*m_upperLimit; - // findex vector for variables. see the LCP solver interface for a - // description of what this does. this is set to -1 on entry. - // note that the returned indexes are relative to the first index of - // the constraint. - int *findex; // number of solver iterations int m_numIterations; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp b/extern/bullet/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h b/extern/bullet/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h rename to extern/bullet/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp b/extern/bullet/src/BulletDynamics/Dynamics/Bullet-C-API.cpp similarity index 100% rename from extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp rename to extern/bullet/src/BulletDynamics/Dynamics/Bullet-C-API.cpp diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h b/extern/bullet/src/BulletDynamics/Dynamics/btActionInterface.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h rename to extern/bullet/src/BulletDynamics/Dynamics/btActionInterface.h diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp similarity index 94% rename from extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp rename to extern/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 361a054ec69c..b9944c138b05 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -374,7 +374,7 @@ void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) void btDiscreteDynamicsWorld::synchronizeMotionStates() { - BT_PROFILE("synchronizeMotionStates"); +// BT_PROFILE("synchronizeMotionStates"); if (m_synchronizeAllMotionStates) { //iterate over all collision objects @@ -402,7 +402,6 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, { startProfiling(timeStep); - BT_PROFILE("stepSimulation"); int numSimulationSubSteps = 0; @@ -539,7 +538,7 @@ btVector3 btDiscreteDynamicsWorld::getGravity () const return m_gravity; } -void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) +void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) { btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask); } @@ -578,14 +577,14 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) } 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); + int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); + int collisionFilterMask = isDynamic? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); addCollisionObject(body,collisionFilterGroup,collisionFilterMask); } } -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask) +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) { if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) { @@ -843,6 +842,9 @@ class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConve btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + if(!m_dispatcher->needsCollision(m_me, otherObj)) + return false; + //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179 if (m_dispatcher->needsResponse(m_me,otherObj)) { @@ -878,25 +880,12 @@ class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConve int gNumClampedCcdMotions=0; -void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) +void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep) { - BT_PROFILE("createPredictiveContacts"); - - { - BT_PROFILE("release predictive contact manifolds"); - - for (int i=0;im_dispatcher1->releaseManifold(manifold); - } - m_predictiveManifolds.clear(); - } - btTransform predictedTrans; - for ( int i=0;isetHitFraction(1.f); if (body->isActive() && (!body->isStaticOrKinematicObject())) @@ -953,7 +942,9 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject); + btMutexLock( &m_predictiveManifoldsMutex ); m_predictiveManifolds.push_back(manifold); + btMutexUnlock( &m_predictiveManifoldsMutex ); btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec; btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB; @@ -964,7 +955,7 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) int index = manifold->addManifoldPoint(newPoint, isPredictive); btManifoldPoint& pt = manifold->getContactPoint(index); pt.m_combinedRestitution = 0; - pt.m_combinedFriction = btManifoldResult::calculateCombinedFriction(body,sweepResults.m_hitCollisionObject); + pt.m_combinedFriction = gCalculateCombinedFrictionCallback(body,sweepResults.m_hitCollisionObject); pt.m_positionWorldOnA = body->getWorldTransform().getOrigin(); pt.m_positionWorldOnB = worldPointB; @@ -974,13 +965,35 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) } } } -void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) + +void btDiscreteDynamicsWorld::releasePredictiveContacts() +{ + BT_PROFILE( "release predictive contact manifolds" ); + + for ( int i = 0; i < m_predictiveManifolds.size(); i++ ) + { + btPersistentManifold* manifold = m_predictiveManifolds[ i ]; + this->m_dispatcher1->releaseManifold( manifold ); + } + m_predictiveManifolds.clear(); +} + +void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) +{ + BT_PROFILE("createPredictiveContacts"); + releasePredictiveContacts(); + if (m_nonStaticRigidBodies.size() > 0) + { + createPredictiveContactsInternal( &m_nonStaticRigidBodies[ 0 ], m_nonStaticRigidBodies.size(), timeStep ); + } +} + +void btDiscreteDynamicsWorld::integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep ) { - BT_PROFILE("integrateTransforms"); btTransform predictedTrans; - for ( int i=0;isetHitFraction(1.f); if (body->isActive() && (!body->isStaticOrKinematicObject())) @@ -1080,7 +1093,17 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) } - ///this should probably be switched on by default, but it is not well tested yet +} + +void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) +{ + BT_PROFILE("integrateTransforms"); + if (m_nonStaticRigidBodies.size() > 0) + { + integrateTransformsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep); + } + + ///this should probably be switched on by default, but it is not well tested yet if (m_applySpeculativeContactRestitution) { BT_PROFILE("apply speculative contact restitution"); @@ -1093,7 +1116,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) for (int p=0;pgetNumContacts();p++) { const btManifoldPoint& pt = manifold->getContactPoint(p); - btScalar combinedRestitution = btManifoldResult::calculateCombinedRestitution(body0, body1); + btScalar combinedRestitution = gCalculateCombinedRestitutionCallback(body0, body1); if (combinedRestitution>0 && pt.m_appliedImpulse != 0.f) //if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f) @@ -1114,14 +1137,12 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) } } } - } - void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { BT_PROFILE("predictUnconstraintMotion"); @@ -1324,9 +1345,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) 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)); + if (minTh <= maxTh) + { + 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); @@ -1493,6 +1517,9 @@ void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serialize worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse; + // Fill padding with zeros to appease msan. + memset(worldInfo->m_solverInfo.m_padding, 0, sizeof(worldInfo->m_solverInfo.m_padding)); + #ifdef BT_USE_DOUBLE_PRECISION const char* structType = "btDynamicsWorldDoubleData"; #else//BT_USE_DOUBLE_PRECISION @@ -1512,6 +1539,8 @@ void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) serializeRigidBodies(serializer); + serializeContactManifolds(serializer); + serializer->finishSerialization(); } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h similarity index 90% rename from extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h rename to extern/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index dd3d1c3660ca..b0d19f48a371 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -30,6 +30,7 @@ class btIDebugDraw; struct InplaceSolverIslandCallback; #include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btThreads.h" ///btDiscreteDynamicsWorld provides discrete rigid body simulation @@ -68,9 +69,11 @@ ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorld : public btDynamicsWorld bool m_latencyMotionStateInterpolation; btAlignedObjectArray m_predictiveManifolds; + btSpinMutex m_predictiveManifoldsMutex; // used to synchronize threads creating predictive contacts virtual void predictUnconstraintMotion(btScalar timeStep); + void integrateTransformsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep ); // can be called in parallel virtual void integrateTransforms(btScalar timeStep); virtual void calculateSimulationIslands(); @@ -85,7 +88,9 @@ ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorld : public btDynamicsWorld virtual void internalSingleStepSimulation( btScalar timeStep); - void createPredictiveContacts(btScalar timeStep); + void releasePredictiveContacts(); + void createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep ); // can be called in parallel + virtual void createPredictiveContacts(btScalar timeStep); virtual void saveKinematicState(btScalar timeStep); @@ -139,11 +144,11 @@ ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorld : public btDynamicsWorld virtual btVector3 getGravity () const; - virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + virtual void addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup=btBroadphaseProxy::StaticFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); virtual void addRigidBody(btRigidBody* body); - virtual void addRigidBody(btRigidBody* body, short group, short mask); + virtual void addRigidBody(btRigidBody* body, int group, int mask); virtual void removeRigidBody(btRigidBody* body); diff --git a/extern/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp b/extern/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp new file mode 100644 index 000000000000..d705bf238174 --- /dev/null +++ b/extern/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp @@ -0,0 +1,278 @@ +/* +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 "btDiscreteDynamicsWorldMt.h" + +//collision detection +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "btSimulationIslandManagerMt.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/btGeneric6DofSpring2Constraint.h" +#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" +#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" + + +#include "LinearMath/btIDebugDraw.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + + +#include "BulletDynamics/Dynamics/btActionInterface.h" +#include "LinearMath/btQuickprof.h" +#include "LinearMath/btMotionState.h" + +#include "LinearMath/btSerializer.h" + + + +/// +/// btConstraintSolverPoolMt +/// + +btConstraintSolverPoolMt::ThreadSolver* btConstraintSolverPoolMt::getAndLockThreadSolver() +{ + int i = 0; +#if BT_THREADSAFE + i = btGetCurrentThreadIndex() % m_solvers.size(); +#endif // #if BT_THREADSAFE + while ( true ) + { + ThreadSolver& solver = m_solvers[ i ]; + if ( solver.mutex.tryLock() ) + { + return &solver; + } + // failed, try the next one + i = ( i + 1 ) % m_solvers.size(); + } + return NULL; +} + +void btConstraintSolverPoolMt::init( btConstraintSolver** solvers, int numSolvers ) +{ + m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER; + m_solvers.resize( numSolvers ); + for ( int i = 0; i < numSolvers; ++i ) + { + m_solvers[ i ].solver = solvers[ i ]; + } + if ( numSolvers > 0 ) + { + m_solverType = solvers[ 0 ]->getSolverType(); + } +} + +// create the solvers for me +btConstraintSolverPoolMt::btConstraintSolverPoolMt( int numSolvers ) +{ + btAlignedObjectArray solvers; + solvers.reserve( numSolvers ); + for ( int i = 0; i < numSolvers; ++i ) + { + btConstraintSolver* solver = new btSequentialImpulseConstraintSolver(); + solvers.push_back( solver ); + } + init( &solvers[ 0 ], numSolvers ); +} + +// pass in fully constructed solvers (destructor will delete them) +btConstraintSolverPoolMt::btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers ) +{ + init( solvers, numSolvers ); +} + +btConstraintSolverPoolMt::~btConstraintSolverPoolMt() +{ + // delete all solvers + for ( int i = 0; i < m_solvers.size(); ++i ) + { + ThreadSolver& solver = m_solvers[ i ]; + delete solver.solver; + solver.solver = NULL; + } +} + +///solve a group of constraints +btScalar btConstraintSolverPoolMt::solveGroup( btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifolds, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& info, + btIDebugDraw* debugDrawer, + btDispatcher* dispatcher +) +{ + ThreadSolver* ts = getAndLockThreadSolver(); + ts->solver->solveGroup( bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher ); + ts->mutex.unlock(); + return 0.0f; +} + +void btConstraintSolverPoolMt::reset() +{ + for ( int i = 0; i < m_solvers.size(); ++i ) + { + ThreadSolver& solver = m_solvers[ i ]; + solver.mutex.lock(); + solver.solver->reset(); + solver.mutex.unlock(); + } +} + + +/// +/// btDiscreteDynamicsWorldMt +/// + +btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, + btBroadphaseInterface* pairCache, + btConstraintSolverPoolMt* constraintSolver, + btConstraintSolver* constraintSolverMt, + btCollisionConfiguration* collisionConfiguration +) +: btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) +{ + if (m_ownsIslandManager) + { + m_islandManager->~btSimulationIslandManager(); + btAlignedFree( m_islandManager); + } + { + void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt),16); + btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt(); + im->setMinimumSolverBatchSize( m_solverInfo.m_minimumSolverBatchSize ); + m_islandManager = im; + } + m_constraintSolverMt = constraintSolverMt; +} + + +btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt() +{ +} + + +void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo) +{ + BT_PROFILE("solveConstraints"); + + m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); + + /// solve all the constraints for this island + btSimulationIslandManagerMt* im = static_cast(m_islandManager); + btSimulationIslandManagerMt::SolverParams solverParams; + solverParams.m_solverPool = m_constraintSolver; + solverParams.m_solverMt = m_constraintSolverMt; + solverParams.m_solverInfo = &solverInfo; + solverParams.m_debugDrawer = m_debugDrawer; + solverParams.m_dispatcher = getCollisionWorld()->getDispatcher(); + im->buildAndProcessIslands( getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams ); + + m_constraintSolver->allSolved(solverInfo, m_debugDrawer); +} + + +struct UpdaterUnconstrainedMotion : public btIParallelForBody +{ + btScalar timeStep; + btRigidBody** rigidBodies; + + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + for ( int i = iBegin; i < iEnd; ++i ) + { + btRigidBody* body = rigidBodies[ i ]; + if ( !body->isStaticOrKinematicObject() ) + { + //don't integrate/update velocities here, it happens in the constraint solver + body->applyDamping( timeStep ); + body->predictIntegratedTransform( timeStep, body->getInterpolationWorldTransform() ); + } + } + } +}; + + +void btDiscreteDynamicsWorldMt::predictUnconstraintMotion( btScalar timeStep ) +{ + BT_PROFILE( "predictUnconstraintMotion" ); + if ( m_nonStaticRigidBodies.size() > 0 ) + { + UpdaterUnconstrainedMotion update; + update.timeStep = timeStep; + update.rigidBodies = &m_nonStaticRigidBodies[ 0 ]; + int grainSize = 50; // num of iterations per task for task scheduler + btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update ); + } +} + + +void btDiscreteDynamicsWorldMt::createPredictiveContacts( btScalar timeStep ) +{ + BT_PROFILE( "createPredictiveContacts" ); + releasePredictiveContacts(); + if ( m_nonStaticRigidBodies.size() > 0 ) + { + UpdaterCreatePredictiveContacts update; + update.world = this; + update.timeStep = timeStep; + update.rigidBodies = &m_nonStaticRigidBodies[ 0 ]; + int grainSize = 50; // num of iterations per task for task scheduler + btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update ); + } +} + + +void btDiscreteDynamicsWorldMt::integrateTransforms( btScalar timeStep ) +{ + BT_PROFILE( "integrateTransforms" ); + if ( m_nonStaticRigidBodies.size() > 0 ) + { + UpdaterIntegrateTransforms update; + update.world = this; + update.timeStep = timeStep; + update.rigidBodies = &m_nonStaticRigidBodies[ 0 ]; + int grainSize = 50; // num of iterations per task for task scheduler + btParallelFor( 0, m_nonStaticRigidBodies.size(), grainSize, update ); + } +} + + +int btDiscreteDynamicsWorldMt::stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep ) +{ + int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep); + if (btITaskScheduler* scheduler = btGetTaskScheduler()) + { + // tell Bullet's threads to sleep, so other threads can run + scheduler->sleepWorkerThreadsHint(); + } + return numSubSteps; +} diff --git a/extern/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h b/extern/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h new file mode 100644 index 000000000000..667fe5800e51 --- /dev/null +++ b/extern/bullet/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h @@ -0,0 +1,136 @@ +/* +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. +*/ + + +#ifndef BT_DISCRETE_DYNAMICS_WORLD_MT_H +#define BT_DISCRETE_DYNAMICS_WORLD_MT_H + +#include "btDiscreteDynamicsWorld.h" +#include "btSimulationIslandManagerMt.h" +#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" + + +/// +/// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them. +/// +/// Each solver in the pool is protected by a mutex. When solveGroup is called from a thread, +/// the pool looks for a solver that isn't being used by another thread, locks it, and dispatches the +/// call to the solver. +/// So long as there are at least as many solvers as there are hardware threads, it should never need to +/// spin wait. +/// +class btConstraintSolverPoolMt : public btConstraintSolver +{ +public: + // create the solvers for me + explicit btConstraintSolverPoolMt( int numSolvers ); + + // pass in fully constructed solvers (destructor will delete them) + btConstraintSolverPoolMt( btConstraintSolver** solvers, int numSolvers ); + + virtual ~btConstraintSolverPoolMt(); + + ///solve a group of constraints + virtual btScalar solveGroup( btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifolds, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& info, + btIDebugDraw* debugDrawer, + btDispatcher* dispatcher + ) BT_OVERRIDE; + + virtual void reset() BT_OVERRIDE; + virtual btConstraintSolverType getSolverType() const BT_OVERRIDE { return m_solverType; } + +private: + const static size_t kCacheLineSize = 128; + struct ThreadSolver + { + btConstraintSolver* solver; + btSpinMutex mutex; + char _cachelinePadding[ kCacheLineSize - sizeof( btSpinMutex ) - sizeof( void* ) ]; // keep mutexes from sharing a cache line + }; + btAlignedObjectArray m_solvers; + btConstraintSolverType m_solverType; + + ThreadSolver* getAndLockThreadSolver(); + void init( btConstraintSolver** solvers, int numSolvers ); +}; + + + +/// +/// btDiscreteDynamicsWorldMt -- a version of DiscreteDynamicsWorld with some minor changes to support +/// solving simulation islands on multiple threads. +/// +/// Should function exactly like btDiscreteDynamicsWorld. +/// Also 3 methods that iterate over all of the rigidbodies can run in parallel: +/// - predictUnconstraintMotion +/// - integrateTransforms +/// - createPredictiveContacts +/// +ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld +{ +protected: + btConstraintSolver* m_constraintSolverMt; + + virtual void solveConstraints(btContactSolverInfo& solverInfo) BT_OVERRIDE; + + virtual void predictUnconstraintMotion( btScalar timeStep ) BT_OVERRIDE; + + struct UpdaterCreatePredictiveContacts : public btIParallelForBody + { + btScalar timeStep; + btRigidBody** rigidBodies; + btDiscreteDynamicsWorldMt* world; + + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + world->createPredictiveContactsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep ); + } + }; + virtual void createPredictiveContacts( btScalar timeStep ) BT_OVERRIDE; + + struct UpdaterIntegrateTransforms : public btIParallelForBody + { + btScalar timeStep; + btRigidBody** rigidBodies; + btDiscreteDynamicsWorldMt* world; + + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + world->integrateTransformsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep ); + } + }; + virtual void integrateTransforms( btScalar timeStep ) BT_OVERRIDE; + +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, + btBroadphaseInterface* pairCache, + btConstraintSolverPoolMt* constraintSolver, // Note this should be a solver-pool for multi-threading + btConstraintSolver* constraintSolverMt, // single multi-threaded solver for large islands (or NULL) + btCollisionConfiguration* collisionConfiguration + ); + virtual ~btDiscreteDynamicsWorldMt(); + + virtual int stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep ) BT_OVERRIDE; +}; + +#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h similarity index 96% rename from extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h rename to extern/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 35dd1400fe7e..42d8fc0de3f6 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -34,7 +34,8 @@ enum btDynamicsWorldType BT_DISCRETE_DYNAMICS_WORLD=2, BT_CONTINUOUS_DYNAMICS_WORLD=3, BT_SOFT_RIGID_DYNAMICS_WORLD=4, - BT_GPU_DYNAMICS_WORLD=5 + BT_GPU_DYNAMICS_WORLD=5, + BT_SOFT_MULTIBODY_DYNAMICS_WORLD=6 }; ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. @@ -88,7 +89,7 @@ class btDynamicsWorld : public btCollisionWorld virtual void addRigidBody(btRigidBody* body) = 0; - virtual void addRigidBody(btRigidBody* body, short group, short mask) = 0; + virtual void addRigidBody(btRigidBody* body, int group, int mask) = 0; virtual void removeRigidBody(btRigidBody* body) = 0; @@ -134,6 +135,11 @@ class btDynamicsWorld : public btCollisionWorld return m_solverInfo; } + const btContactSolverInfo& getSolverInfo() const + { + return m_solverInfo; + } + ///obsolete, use addAction instead. virtual void addVehicle(btActionInterface* vehicle) {(void)vehicle;} diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp similarity index 98% rename from extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp rename to extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp index a7882684bf1a..a9f274962120 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -79,6 +79,8 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& //moved to btCollisionObject m_friction = constructionInfo.m_friction; m_rollingFriction = constructionInfo.m_rollingFriction; + m_spinningFriction = constructionInfo.m_spinningFriction; + m_restitution = constructionInfo.m_restitution; setCollisionShape( constructionInfo.m_collisionShape ); @@ -493,6 +495,11 @@ const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali rbd->m_linearSleepingThreshold=m_linearSleepingThreshold; rbd->m_angularSleepingThreshold = m_angularSleepingThreshold; + // Fill padding with zeros to appease msan. +#ifdef BT_USE_DOUBLE_PRECISION + memset(rbd->m_padding, 0, sizeof(rbd->m_padding)); +#endif + return btRigidBodyDataName; } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.h similarity index 99% rename from extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h rename to extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.h index c2f8c5d64ae0..dbbf9958618c 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet/src/BulletDynamics/Dynamics/btRigidBody.h @@ -135,6 +135,8 @@ class btRigidBody : public btCollisionObject ///the m_rollingFriction prevents rounded shapes, such as spheres, cylinders and capsules from rolling forever. ///See Bullet/Demos/RollingFrictionDemo for usage btScalar m_rollingFriction; + btScalar m_spinningFriction;//torsional friction around contact normal + ///best simulation results using zero restitution. btScalar m_restitution; @@ -158,6 +160,7 @@ class btRigidBody : public btCollisionObject m_angularDamping(btScalar(0.)), m_friction(btScalar(0.5)), m_rollingFriction(btScalar(0)), + m_spinningFriction(btScalar(0)), m_restitution(btScalar(0.)), m_linearSleepingThreshold(btScalar(0.8)), m_angularSleepingThreshold(btScalar(1.f)), diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp similarity index 98% rename from extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp rename to extern/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 35dd38840f60..6f63b87c80ae 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/extern/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -155,7 +155,7 @@ void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) } } -void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask) +void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) { body->setGravity(m_gravity); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h similarity index 97% rename from extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h rename to extern/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index d48d2e39c4d1..44b7e7fb34b2 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/extern/bullet/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -56,7 +56,7 @@ class btSimpleDynamicsWorld : public btDynamicsWorld virtual void addRigidBody(btRigidBody* body); - virtual void addRigidBody(btRigidBody* body, short group, short mask); + virtual void addRigidBody(btRigidBody* body, int group, int mask); virtual void removeRigidBody(btRigidBody* body); diff --git a/extern/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/extern/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp new file mode 100644 index 000000000000..fc54f0ba6eab --- /dev/null +++ b/extern/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp @@ -0,0 +1,723 @@ +/* +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 "LinearMath/btThreads.h" +#include "btSimulationIslandManagerMt.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h" // for s_minimumContactManifoldsForBatching + +//#include +#include "LinearMath/btQuickprof.h" + + +SIMD_FORCE_INLINE int calcBatchCost( int bodies, int manifolds, int constraints ) +{ + // rough estimate of the cost of a batch, used for merging + int batchCost = bodies + 8 * manifolds + 4 * constraints; + return batchCost; +} + + +SIMD_FORCE_INLINE int calcBatchCost( const btSimulationIslandManagerMt::Island* island ) +{ + return calcBatchCost( island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size() ); +} + + +btSimulationIslandManagerMt::btSimulationIslandManagerMt() +{ + m_minimumSolverBatchSize = calcBatchCost(0, 128, 0); + m_batchIslandMinBodyCount = 32; + m_islandDispatch = parallelIslandDispatch; + m_batchIsland = NULL; +} + + +btSimulationIslandManagerMt::~btSimulationIslandManagerMt() +{ + for ( int i = 0; i < m_allocatedIslands.size(); ++i ) + { + delete m_allocatedIslands[ i ]; + } + m_allocatedIslands.resize( 0 ); + m_activeIslands.resize( 0 ); + m_freeIslands.resize( 0 ); +} + + +inline int getIslandId(const btPersistentManifold* lhs) +{ + const btCollisionObject* rcolObj0 = static_cast(lhs->getBody0()); + const btCollisionObject* rcolObj1 = static_cast(lhs->getBody1()); + int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag(); + return islandId; +} + + +SIMD_FORCE_INLINE int btGetConstraintIslandId( const btTypedConstraint* lhs ) +{ + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); + return islandId; +} + +/// function object that routes calls to operator< +class IslandBatchSizeSortPredicate +{ +public: + bool operator() ( const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs ) const + { + int lCost = calcBatchCost( lhs ); + int rCost = calcBatchCost( rhs ); + return lCost > rCost; + } +}; + + +class IslandBodyCapacitySortPredicate +{ +public: + bool operator() ( const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs ) const + { + return lhs->bodyArray.capacity() > rhs->bodyArray.capacity(); + } +}; + + +void btSimulationIslandManagerMt::Island::append( const Island& other ) +{ + // append bodies + for ( int i = 0; i < other.bodyArray.size(); ++i ) + { + bodyArray.push_back( other.bodyArray[ i ] ); + } + // append manifolds + for ( int i = 0; i < other.manifoldArray.size(); ++i ) + { + manifoldArray.push_back( other.manifoldArray[ i ] ); + } + // append constraints + for ( int i = 0; i < other.constraintArray.size(); ++i ) + { + constraintArray.push_back( other.constraintArray[ i ] ); + } +} + + +bool btIsBodyInIsland( const btSimulationIslandManagerMt::Island& island, const btCollisionObject* obj ) +{ + for ( int i = 0; i < island.bodyArray.size(); ++i ) + { + if ( island.bodyArray[ i ] == obj ) + { + return true; + } + } + return false; +} + + +void btSimulationIslandManagerMt::initIslandPools() +{ + // reset island pools + int numElem = getUnionFind().getNumElements(); + m_lookupIslandFromId.resize( numElem ); + for ( int i = 0; i < m_lookupIslandFromId.size(); ++i ) + { + m_lookupIslandFromId[ i ] = NULL; + } + m_activeIslands.resize( 0 ); + m_freeIslands.resize( 0 ); + // check whether allocated islands are sorted by body capacity (largest to smallest) + int lastCapacity = 0; + bool isSorted = true; + for ( int i = 0; i < m_allocatedIslands.size(); ++i ) + { + Island* island = m_allocatedIslands[ i ]; + int cap = island->bodyArray.capacity(); + if ( cap > lastCapacity ) + { + isSorted = false; + break; + } + lastCapacity = cap; + } + if ( !isSorted ) + { + m_allocatedIslands.quickSort( IslandBodyCapacitySortPredicate() ); + } + + m_batchIsland = NULL; + // mark all islands free (but avoid deallocation) + for ( int i = 0; i < m_allocatedIslands.size(); ++i ) + { + Island* island = m_allocatedIslands[ i ]; + island->bodyArray.resize( 0 ); + island->manifoldArray.resize( 0 ); + island->constraintArray.resize( 0 ); + island->id = -1; + island->isSleeping = true; + m_freeIslands.push_back( island ); + } +} + + +btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland( int id ) +{ + Island* island = m_lookupIslandFromId[ id ]; + if ( island == NULL ) + { + // search for existing island + for ( int i = 0; i < m_activeIslands.size(); ++i ) + { + if ( m_activeIslands[ i ]->id == id ) + { + island = m_activeIslands[ i ]; + break; + } + } + m_lookupIslandFromId[ id ] = island; + } + return island; +} + + +btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland( int id, int numBodies ) +{ + Island* island = NULL; + int allocSize = numBodies; + if ( numBodies < m_batchIslandMinBodyCount ) + { + if ( m_batchIsland ) + { + island = m_batchIsland; + m_lookupIslandFromId[ id ] = island; + // if we've made a large enough batch, + if ( island->bodyArray.size() + numBodies >= m_batchIslandMinBodyCount ) + { + // next time start a new batch + m_batchIsland = NULL; + } + return island; + } + else + { + // need to allocate a batch island + allocSize = m_batchIslandMinBodyCount * 2; + } + } + btAlignedObjectArray& freeIslands = m_freeIslands; + + // search for free island + if ( freeIslands.size() > 0 ) + { + // try to reuse a previously allocated island + int iFound = freeIslands.size(); + // linear search for smallest island that can hold our bodies + for ( int i = freeIslands.size() - 1; i >= 0; --i ) + { + if ( freeIslands[ i ]->bodyArray.capacity() >= allocSize ) + { + iFound = i; + island = freeIslands[ i ]; + island->id = id; + break; + } + } + // if found, shrink array while maintaining ordering + if ( island ) + { + int iDest = iFound; + int iSrc = iDest + 1; + while ( iSrc < freeIslands.size() ) + { + freeIslands[ iDest++ ] = freeIslands[ iSrc++ ]; + } + freeIslands.pop_back(); + } + } + if ( island == NULL ) + { + // no free island found, allocate + island = new Island(); // TODO: change this to use the pool allocator + island->id = id; + island->bodyArray.reserve( allocSize ); + m_allocatedIslands.push_back( island ); + } + m_lookupIslandFromId[ id ] = island; + if ( numBodies < m_batchIslandMinBodyCount ) + { + m_batchIsland = island; + } + m_activeIslands.push_back( island ); + return island; +} + + +void btSimulationIslandManagerMt::buildIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld ) +{ + + BT_PROFILE("buildIslands"); + + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + + //we are going to sort the unionfind array, and store the element id in the size + //afterwards, we clean unionfind, to make sure no-one uses it anymore + + getUnionFind().sortIslands(); + int numElem = getUnionFind().getNumElements(); + + int endIslandIndex=1; + int startIslandIndex; + + //update the sleeping state for bodies, if all are sleeping + for ( startIslandIndex=0;startIslandIndexgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) + { +// printf("error in island management\n"); + } + + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + if (colObj0->getIslandTag() == islandId) + { + if (colObj0->getActivationState()== ACTIVE_TAG || + colObj0->getActivationState()== DISABLE_DEACTIVATION) + { + allSleeping = false; + break; + } + } + } + + if (allSleeping) + { + int idx; + for (idx=startIslandIndex;idxgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) + { +// printf("error in island management\n"); + } + + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + + if (colObj0->getIslandTag() == islandId) + { + colObj0->setActivationState( ISLAND_SLEEPING ); + } + } + } else + { + + int idx; + for (idx=startIslandIndex;idxgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) + { +// printf("error in island management\n"); + } + + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + + if (colObj0->getIslandTag() == islandId) + { + if ( colObj0->getActivationState() == ISLAND_SLEEPING) + { + colObj0->setActivationState( WANTS_DEACTIVATION); + colObj0->setDeactivationTime(0.f); + } + } + } + } + } +} + + +void btSimulationIslandManagerMt::addBodiesToIslands( btCollisionWorld* collisionWorld ) +{ + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + int endIslandIndex = 1; + int startIslandIndex; + int numElem = getUnionFind().getNumElements(); + + // create explicit islands and add bodies to each + for ( startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex ) + { + int islandId = getUnionFind().getElement( startIslandIndex ).m_id; + + // find end index + for ( endIslandIndex = startIslandIndex; ( endIslandIndex < numElem ) && ( getUnionFind().getElement( endIslandIndex ).m_id == islandId ); endIslandIndex++ ) + { + } + // check if island is sleeping + bool islandSleeping = true; + for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ ) + { + int i = getUnionFind().getElement( iElem ).m_sz; + btCollisionObject* colObj = collisionObjects[ i ]; + if ( colObj->isActive() ) + { + islandSleeping = false; + } + } + if ( !islandSleeping ) + { + // want to count the number of bodies before allocating the island to optimize memory usage of the Island structures + int numBodies = endIslandIndex - startIslandIndex; + Island* island = allocateIsland( islandId, numBodies ); + island->isSleeping = false; + + // add bodies to island + for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ ) + { + int i = getUnionFind().getElement( iElem ).m_sz; + btCollisionObject* colObj = collisionObjects[ i ]; + island->bodyArray.push_back( colObj ); + } + } + } + +} + + +void btSimulationIslandManagerMt::addManifoldsToIslands( btDispatcher* dispatcher ) +{ + // walk all the manifolds, activating bodies touched by kinematic objects, and add each manifold to its Island + int maxNumManifolds = dispatcher->getNumManifolds(); + for ( int i = 0; i < maxNumManifolds; i++ ) + { + btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal( i ); + + const btCollisionObject* colObj0 = static_cast( manifold->getBody0() ); + const btCollisionObject* colObj1 = static_cast( manifold->getBody1() ); + + ///@todo: check sleeping conditions! + if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) || + ( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) ) + { + + //kinematic objects don't merge islands, but wake up all connected objects + if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING ) + { + if ( colObj0->hasContactResponse() ) + colObj1->activate(); + } + if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING ) + { + if ( colObj1->hasContactResponse() ) + colObj0->activate(); + } + //filtering for response + if ( dispatcher->needsResponse( colObj0, colObj1 ) ) + { + // scatter manifolds into various islands + int islandId = getIslandId( manifold ); + // if island not sleeping, + if ( Island* island = getIsland( islandId ) ) + { + island->manifoldArray.push_back( manifold ); + } + } + } + } +} + + +void btSimulationIslandManagerMt::addConstraintsToIslands( btAlignedObjectArray& constraints ) +{ + // walk constraints + for ( int i = 0; i < constraints.size(); i++ ) + { + // scatter constraints into various islands + btTypedConstraint* constraint = constraints[ i ]; + if ( constraint->isEnabled() ) + { + int islandId = btGetConstraintIslandId( constraint ); + // if island is not sleeping, + if ( Island* island = getIsland( islandId ) ) + { + island->constraintArray.push_back( constraint ); + } + } + } +} + + +void btSimulationIslandManagerMt::mergeIslands() +{ + // sort islands in order of decreasing batch size + m_activeIslands.quickSort( IslandBatchSizeSortPredicate() ); + + // merge small islands to satisfy minimum batch size + // find first small batch island + int destIslandIndex = m_activeIslands.size(); + for ( int i = 0; i < m_activeIslands.size(); ++i ) + { + Island* island = m_activeIslands[ i ]; + int batchSize = calcBatchCost( island ); + if ( batchSize < m_minimumSolverBatchSize ) + { + destIslandIndex = i; + break; + } + } + int lastIndex = m_activeIslands.size() - 1; + while ( destIslandIndex < lastIndex ) + { + // merge islands from the back of the list + Island* island = m_activeIslands[ destIslandIndex ]; + int numBodies = island->bodyArray.size(); + int numManifolds = island->manifoldArray.size(); + int numConstraints = island->constraintArray.size(); + int firstIndex = lastIndex; + // figure out how many islands we want to merge and find out how many bodies, manifolds and constraints we will have + while ( true ) + { + Island* src = m_activeIslands[ firstIndex ]; + numBodies += src->bodyArray.size(); + numManifolds += src->manifoldArray.size(); + numConstraints += src->constraintArray.size(); + int batchCost = calcBatchCost( numBodies, numManifolds, numConstraints ); + if ( batchCost >= m_minimumSolverBatchSize ) + { + break; + } + if ( firstIndex - 1 == destIslandIndex ) + { + break; + } + firstIndex--; + } + // reserve space for these pointers to minimize reallocation + island->bodyArray.reserve( numBodies ); + island->manifoldArray.reserve( numManifolds ); + island->constraintArray.reserve( numConstraints ); + // merge islands + for ( int i = firstIndex; i <= lastIndex; ++i ) + { + island->append( *m_activeIslands[ i ] ); + } + // shrink array to exclude the islands that were merged from + m_activeIslands.resize( firstIndex ); + lastIndex = firstIndex - 1; + destIslandIndex++; + } +} + + +void btSimulationIslandManagerMt::solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams) +{ + btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[ 0 ] : NULL; + btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[ 0 ] : NULL; + solver->solveGroup( &island.bodyArray[ 0 ], + island.bodyArray.size(), + manifolds, + island.manifoldArray.size(), + constraintsPtr, + island.constraintArray.size(), + *solverParams.m_solverInfo, + solverParams.m_debugDrawer, + solverParams.m_dispatcher + ); +} + + +void btSimulationIslandManagerMt::serialIslandDispatch( btAlignedObjectArray* islandsPtr, const SolverParams& solverParams ) +{ + BT_PROFILE( "serialIslandDispatch" ); + // serial dispatch + btAlignedObjectArray& islands = *islandsPtr; + btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; + for ( int i = 0; i < islands.size(); ++i ) + { + solveIsland(solver, *islands[ i ], solverParams); + } +} + + +struct UpdateIslandDispatcher : public btIParallelForBody +{ + btAlignedObjectArray& m_islandsPtr; + const btSimulationIslandManagerMt::SolverParams& m_solverParams; + + UpdateIslandDispatcher(btAlignedObjectArray& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams) + : m_islandsPtr(islandsPtr), m_solverParams(solverParams) + {} + + void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE + { + btConstraintSolver* solver = m_solverParams.m_solverPool; + for ( int i = iBegin; i < iEnd; ++i ) + { + btSimulationIslandManagerMt::Island* island = m_islandsPtr[ i ]; + btSimulationIslandManagerMt::solveIsland( solver, *island, m_solverParams ); + } + } +}; + + +void btSimulationIslandManagerMt::parallelIslandDispatch( btAlignedObjectArray* islandsPtr, const SolverParams& solverParams ) +{ + BT_PROFILE( "parallelIslandDispatch" ); + // + // if there are islands with many contacts, it may be faster to submit these + // large islands *serially* to a single parallel constraint solver, and then later + // submit the remaining smaller islands in parallel to multiple sequential solvers. + // + // Some task schedulers do not deal well with nested parallelFor loops. One implementation + // of OpenMP was actually slower than doing everything single-threaded. Intel TBB + // on the other hand, seems to do a pretty respectable job with it. + // + // When solving islands in parallel, the worst case performance happens when there + // is one very large island and then perhaps a smattering of very small + // islands -- one worker thread takes the large island and the remaining workers + // tear through the smaller islands and then sit idle waiting for the first worker + // to finish. Solving islands in parallel works best when there are numerous small + // islands, roughly equal in size. + // + // By contrast, the other approach -- the parallel constraint solver -- is only + // able to deliver a worthwhile speedup when the island is large. For smaller islands, + // it is difficult to extract a useful amount of parallelism -- the overhead of grouping + // the constraints into batches and sending the batches to worker threads can nullify + // any gains from parallelism. + // + + UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams); + // We take advantage of the fact the islands are sorted in order of decreasing size + int iBegin = 0; + if (solverParams.m_solverMt) + { + while ( iBegin < islandsPtr->size() ) + { + btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ iBegin ]; + if ( island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching ) + { + // OK to submit the rest of the array in parallel + break; + } + // serial dispatch to parallel solver for large islands (if any) + solveIsland(solverParams.m_solverMt, *island, solverParams); + ++iBegin; + } + } + // parallel dispatch to sequential solvers for rest + btParallelFor( iBegin, islandsPtr->size(), 1, dispatcher ); +} + + +///@todo: this is random access, it can be walked 'cache friendly'! +void btSimulationIslandManagerMt::buildAndProcessIslands( btDispatcher* dispatcher, + btCollisionWorld* collisionWorld, + btAlignedObjectArray& constraints, + const SolverParams& solverParams + ) +{ + BT_PROFILE("buildAndProcessIslands"); + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + + buildIslands(dispatcher,collisionWorld); + + if(!getSplitIslands()) + { + btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer(); + int maxNumManifolds = dispatcher->getNumManifolds(); + + for ( int i = 0; i < maxNumManifolds; i++ ) + { + btPersistentManifold* manifold = manifolds[ i ]; + + const btCollisionObject* colObj0 = static_cast( manifold->getBody0() ); + const btCollisionObject* colObj1 = static_cast( manifold->getBody1() ); + + ///@todo: check sleeping conditions! + if ( ( ( colObj0 ) && colObj0->getActivationState() != ISLAND_SLEEPING ) || + ( ( colObj1 ) && colObj1->getActivationState() != ISLAND_SLEEPING ) ) + { + + //kinematic objects don't merge islands, but wake up all connected objects + if ( colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING ) + { + if ( colObj0->hasContactResponse() ) + colObj1->activate(); + } + if ( colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING ) + { + if ( colObj1->hasContactResponse() ) + colObj0->activate(); + } + } + } + btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[ 0 ] : NULL; + btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; + solver->solveGroup(&collisionObjects[0], + collisionObjects.size(), + manifolds, + maxNumManifolds, + constraintsPtr, + constraints.size(), + *solverParams.m_solverInfo, + solverParams.m_debugDrawer, + solverParams.m_dispatcher + ); + } + else + { + initIslandPools(); + + //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated + addBodiesToIslands( collisionWorld ); + addManifoldsToIslands( dispatcher ); + addConstraintsToIslands( constraints ); + + // m_activeIslands array should now contain all non-sleeping Islands, and each Island should + // have all the necessary bodies, manifolds and constraints. + + // if we want to merge islands with small batch counts, + if ( m_minimumSolverBatchSize > 1 ) + { + mergeIslands(); + } + // dispatch islands to solver + m_islandDispatch( &m_activeIslands, solverParams ); + } +} diff --git a/extern/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h b/extern/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h new file mode 100644 index 000000000000..563577a6f430 --- /dev/null +++ b/extern/bullet/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h @@ -0,0 +1,114 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SIMULATION_ISLAND_MANAGER_MT_H +#define BT_SIMULATION_ISLAND_MANAGER_MT_H + +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" + +class btTypedConstraint; +class btConstraintSolver; +struct btContactSolverInfo; +class btIDebugDraw; + +/// +/// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager +/// Splits the world up into islands which can be solved in parallel. +/// In order to solve islands in parallel, an IslandDispatch function +/// must be provided which will dispatch calls to multiple threads. +/// The amount of parallelism that can be achieved depends on the number +/// of islands. If only a single island exists, then no parallelism is +/// possible. +/// +class btSimulationIslandManagerMt : public btSimulationIslandManager +{ +public: + struct Island + { + // a simulation island consisting of bodies, manifolds and constraints, + // to be passed into a constraint solver. + btAlignedObjectArray bodyArray; + btAlignedObjectArray manifoldArray; + btAlignedObjectArray constraintArray; + int id; // island id + bool isSleeping; + + void append( const Island& other ); // add bodies, manifolds, constraints to my own + }; + struct SolverParams + { + btConstraintSolver* m_solverPool; + btConstraintSolver* m_solverMt; + btContactSolverInfo* m_solverInfo; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; + }; + static void solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams); + + typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray* islands, const SolverParams& solverParams ); + static void serialIslandDispatch( btAlignedObjectArray* islandsPtr, const SolverParams& solverParams ); + static void parallelIslandDispatch( btAlignedObjectArray* islandsPtr, const SolverParams& solverParams ); +protected: + btAlignedObjectArray m_allocatedIslands; // owner of all Islands + btAlignedObjectArray m_activeIslands; // islands actively in use + btAlignedObjectArray m_freeIslands; // islands ready to be reused + btAlignedObjectArray m_lookupIslandFromId; // big lookup table to map islandId to Island pointer + Island* m_batchIsland; + int m_minimumSolverBatchSize; + int m_batchIslandMinBodyCount; + IslandDispatchFunc m_islandDispatch; + + Island* getIsland( int id ); + virtual Island* allocateIsland( int id, int numBodies ); + virtual void initIslandPools(); + virtual void addBodiesToIslands( btCollisionWorld* collisionWorld ); + virtual void addManifoldsToIslands( btDispatcher* dispatcher ); + virtual void addConstraintsToIslands( btAlignedObjectArray& constraints ); + virtual void mergeIslands(); + +public: + btSimulationIslandManagerMt(); + virtual ~btSimulationIslandManagerMt(); + + virtual void buildAndProcessIslands( btDispatcher* dispatcher, + btCollisionWorld* collisionWorld, + btAlignedObjectArray& constraints, + const SolverParams& solverParams + ); + + virtual void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld); + + int getMinimumSolverBatchSize() const + { + return m_minimumSolverBatchSize; + } + void setMinimumSolverBatchSize( int sz ) + { + m_minimumSolverBatchSize = sz; + } + IslandDispatchFunc getIslandDispatchFunction() const + { + return m_islandDispatch; + } + // allow users to set their own dispatch function for multithreaded dispatch + void setIslandDispatchFunction( IslandDispatchFunc func ) + { + m_islandDispatch = func; + } +}; + + +#endif //BT_SIMULATION_ISLAND_MANAGER_H + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp similarity index 86% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp index aea8c6837326..0493cdb024c7 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -1,1946 +1,2093 @@ -/* - * PURPOSE: - * Class representing an articulated rigid body. Stores the body's - * current state, allows forces and torques to be set, handles - * timestepping and implements Featherstone's algorithm. - * - * COPYRIGHT: - * Copyright (C) Stephen Thompson, , 2011-2013 - * Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) - * Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements - - 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 "btMultiBody.h" -#include "btMultiBodyLink.h" -#include "btMultiBodyLinkCollider.h" -#include "btMultiBodyJointFeedback.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btSerializer.h" -// #define INCLUDE_GYRO_TERM - -///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals -bool gJointFeedbackInWorldSpace = false; -bool gJointFeedbackInJointFrame = false; - -namespace { - const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) - const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds -} - -namespace { - void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame - const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates - const btVector3 &top_in, // top part of input vector - const btVector3 &bottom_in, // bottom part of input vector - btVector3 &top_out, // top part of output vector - btVector3 &bottom_out) // bottom part of output vector - { - top_out = rotation_matrix * top_in; - bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in; - } - -/* - void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix, - const btVector3 &displacement, - const btVector3 &top_in, - const btVector3 &bottom_in, - btVector3 &top_out, - btVector3 &bottom_out) - { - top_out = rotation_matrix.transpose() * top_in; - bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in)); - } - - btScalar SpatialDotProduct(const btVector3 &a_top, - const btVector3 &a_bottom, - const btVector3 &b_top, - const btVector3 &b_bottom) - { - return a_bottom.dot(b_top) + a_top.dot(b_bottom); - } - - void SpatialCrossProduct(const btVector3 &a_top, - const btVector3 &a_bottom, - const btVector3 &b_top, - const btVector3 &b_bottom, - btVector3 &top_out, - btVector3 &bottom_out) - { - top_out = a_top.cross(b_top); - bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom); - } -*/ -} - - -// -// Implementation of class btMultiBody -// - -btMultiBody::btMultiBody(int n_links, - btScalar mass, - const btVector3 &inertia, - bool fixedBase, - bool canSleep, - bool /*deprecatedUseMultiDof*/) - : - m_baseCollider(0), - m_baseName(0), - m_basePos(0,0,0), - m_baseQuat(0, 0, 0, 1), - m_baseMass(mass), - m_baseInertia(inertia), - - m_fixedBase(fixedBase), - m_awake(true), - m_canSleep(canSleep), - m_sleepTimer(0), - - m_linearDamping(0.04f), - m_angularDamping(0.04f), - m_useGyroTerm(true), - m_maxAppliedImpulse(1000.f), - m_maxCoordinateVelocity(100.f), - m_hasSelfCollision(true), - __posUpdated(false), - m_dofCount(0), - m_posVarCnt(0), - m_useRK4(false), - m_useGlobalVelocities(false), - m_internalNeedsJointFeedback(false) -{ - m_links.resize(n_links); - m_matrixBuf.resize(n_links + 1); - - - m_baseForce.setValue(0, 0, 0); - m_baseTorque.setValue(0, 0, 0); -} - -btMultiBody::~btMultiBody() -{ -} - -void btMultiBody::setupFixed(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/) -{ - - m_links[i].m_mass = mass; - m_links[i].m_inertiaLocal = inertia; - m_links[i].m_parent = parent; - m_links[i].m_zeroRotParentToThis = rotParentToThis; - m_links[i].m_dVector = thisPivotToThisComOffset; - m_links[i].m_eVector = parentComToThisPivotOffset; - - m_links[i].m_jointType = btMultibodyLink::eFixed; - m_links[i].m_dofCount = 0; - m_links[i].m_posVarCount = 0; - - m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; - - m_links[i].updateCacheMultiDof(); - - updateLinksDofOffsets(); - -} - - -void btMultiBody::setupPrismatic(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &jointAxis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, - bool disableParentCollision) -{ - m_dofCount += 1; - m_posVarCnt += 1; - - m_links[i].m_mass = mass; - m_links[i].m_inertiaLocal = inertia; - m_links[i].m_parent = parent; - m_links[i].m_zeroRotParentToThis = rotParentToThis; - m_links[i].setAxisTop(0, 0., 0., 0.); - m_links[i].setAxisBottom(0, jointAxis); - m_links[i].m_eVector = parentComToThisPivotOffset; - m_links[i].m_dVector = thisPivotToThisComOffset; - m_links[i].m_cachedRotParentToThis = rotParentToThis; - - m_links[i].m_jointType = btMultibodyLink::ePrismatic; - m_links[i].m_dofCount = 1; - m_links[i].m_posVarCount = 1; - m_links[i].m_jointPos[0] = 0.f; - m_links[i].m_jointTorque[0] = 0.f; - - if (disableParentCollision) - m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; - // - - m_links[i].updateCacheMultiDof(); - - updateLinksDofOffsets(); -} - -void btMultiBody::setupRevolute(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &jointAxis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, - bool disableParentCollision) -{ - m_dofCount += 1; - m_posVarCnt += 1; - - m_links[i].m_mass = mass; - m_links[i].m_inertiaLocal = inertia; - m_links[i].m_parent = parent; - m_links[i].m_zeroRotParentToThis = rotParentToThis; - m_links[i].setAxisTop(0, jointAxis); - m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset)); - m_links[i].m_dVector = thisPivotToThisComOffset; - m_links[i].m_eVector = parentComToThisPivotOffset; - - m_links[i].m_jointType = btMultibodyLink::eRevolute; - m_links[i].m_dofCount = 1; - m_links[i].m_posVarCount = 1; - m_links[i].m_jointPos[0] = 0.f; - m_links[i].m_jointTorque[0] = 0.f; - - if (disableParentCollision) - m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; - // - m_links[i].updateCacheMultiDof(); - // - updateLinksDofOffsets(); -} - - - -void btMultiBody::setupSpherical(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, - bool disableParentCollision) -{ - - m_dofCount += 3; - m_posVarCnt += 4; - - m_links[i].m_mass = mass; - m_links[i].m_inertiaLocal = inertia; - m_links[i].m_parent = parent; - m_links[i].m_zeroRotParentToThis = rotParentToThis; - m_links[i].m_dVector = thisPivotToThisComOffset; - m_links[i].m_eVector = parentComToThisPivotOffset; - - m_links[i].m_jointType = btMultibodyLink::eSpherical; - m_links[i].m_dofCount = 3; - m_links[i].m_posVarCount = 4; - m_links[i].setAxisTop(0, 1.f, 0.f, 0.f); - m_links[i].setAxisTop(1, 0.f, 1.f, 0.f); - m_links[i].setAxisTop(2, 0.f, 0.f, 1.f); - m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset)); - m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset)); - m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset)); - m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointPos[3] = 1.f; - m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f; - - - if (disableParentCollision) - m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; - // - m_links[i].updateCacheMultiDof(); - // - updateLinksDofOffsets(); -} - -void btMultiBody::setupPlanar(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &rotationAxis, - const btVector3 &parentComToThisComOffset, - bool disableParentCollision) -{ - - m_dofCount += 3; - m_posVarCnt += 3; - - m_links[i].m_mass = mass; - m_links[i].m_inertiaLocal = inertia; - m_links[i].m_parent = parent; - m_links[i].m_zeroRotParentToThis = rotParentToThis; - m_links[i].m_dVector.setZero(); - m_links[i].m_eVector = parentComToThisComOffset; - - // - static btVector3 vecNonParallelToRotAxis(1, 0, 0); - if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999) - vecNonParallelToRotAxis.setValue(0, 1, 0); - // - - m_links[i].m_jointType = btMultibodyLink::ePlanar; - m_links[i].m_dofCount = 3; - m_links[i].m_posVarCount = 3; - btVector3 n=rotationAxis.normalized(); - m_links[i].setAxisTop(0, n[0],n[1],n[2]); - m_links[i].setAxisTop(1,0,0,0); - m_links[i].setAxisTop(2,0,0,0); - m_links[i].setAxisBottom(0,0,0,0); - btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis); - m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]); - cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0)); - m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]); - m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; - m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f; - - if (disableParentCollision) - m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; - // - m_links[i].updateCacheMultiDof(); - // - updateLinksDofOffsets(); -} - -void btMultiBody::finalizeMultiDof() -{ - m_deltaV.resize(0); - m_deltaV.resize(6 + m_dofCount); - m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") - m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) - - updateLinksDofOffsets(); -} - -int btMultiBody::getParent(int i) const -{ - return m_links[i].m_parent; -} - -btScalar btMultiBody::getLinkMass(int i) const -{ - return m_links[i].m_mass; -} - -const btVector3 & btMultiBody::getLinkInertia(int i) const -{ - return m_links[i].m_inertiaLocal; -} - -btScalar btMultiBody::getJointPos(int i) const -{ - return m_links[i].m_jointPos[0]; -} - -btScalar btMultiBody::getJointVel(int i) const -{ - return m_realBuf[6 + m_links[i].m_dofOffset]; -} - -btScalar * btMultiBody::getJointPosMultiDof(int i) -{ - return &m_links[i].m_jointPos[0]; -} - -btScalar * btMultiBody::getJointVelMultiDof(int i) -{ - return &m_realBuf[6 + m_links[i].m_dofOffset]; -} - -const btScalar * btMultiBody::getJointPosMultiDof(int i) const -{ - return &m_links[i].m_jointPos[0]; -} - -const btScalar * btMultiBody::getJointVelMultiDof(int i) const -{ - return &m_realBuf[6 + m_links[i].m_dofOffset]; -} - - -void btMultiBody::setJointPos(int i, btScalar q) -{ - m_links[i].m_jointPos[0] = q; - m_links[i].updateCacheMultiDof(); -} - -void btMultiBody::setJointPosMultiDof(int i, btScalar *q) -{ - for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos) - m_links[i].m_jointPos[pos] = q[pos]; - - m_links[i].updateCacheMultiDof(); -} - -void btMultiBody::setJointVel(int i, btScalar qdot) -{ - m_realBuf[6 + m_links[i].m_dofOffset] = qdot; -} - -void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot) -{ - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof]; -} - -const btVector3 & btMultiBody::getRVector(int i) const -{ - return m_links[i].m_cachedRVector; -} - -const btQuaternion & btMultiBody::getParentToLocalRot(int i) const -{ - return m_links[i].m_cachedRotParentToThis; -} - -btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const -{ - btVector3 result = local_pos; - while (i != -1) { - // 'result' is in frame i. transform it to frame parent(i) - result += getRVector(i); - result = quatRotate(getParentToLocalRot(i).inverse(),result); - i = getParent(i); - } - - // 'result' is now in the base frame. transform it to world frame - result = quatRotate(getWorldToBaseRot().inverse() ,result); - result += getBasePos(); - - return result; -} - -btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const -{ - if (i == -1) { - // world to base - return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos())); - } else { - // find position in parent frame, then transform to current frame - return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i); - } -} - -btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const -{ - btVector3 result = local_dir; - while (i != -1) { - result = quatRotate(getParentToLocalRot(i).inverse() , result); - i = getParent(i); - } - result = quatRotate(getWorldToBaseRot().inverse() , result); - return result; -} - -btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const -{ - if (i == -1) { - return quatRotate(getWorldToBaseRot(), world_dir); - } else { - return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir)); - } -} - -void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const -{ - int num_links = getNumLinks(); - // Calculates the velocities of each link (and the base) in its local frame - omega[0] = quatRotate(m_baseQuat ,getBaseOmega()); - vel[0] = quatRotate(m_baseQuat ,getBaseVel()); - - for (int i = 0; i < num_links; ++i) { - const int parent = m_links[i].m_parent; - - // transform parent vel into this frame, store in omega[i+1], vel[i+1] - SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector, - omega[parent+1], vel[parent+1], - omega[i+1], vel[i+1]); - - // now add qidot * shat_i - omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0); - vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0); - } -} - -btScalar btMultiBody::getKineticEnergy() const -{ - int num_links = getNumLinks(); - // TODO: would be better not to allocate memory here - btAlignedObjectArray omega;omega.resize(num_links+1); - btAlignedObjectArray vel;vel.resize(num_links+1); - compTreeLinkVelocities(&omega[0], &vel[0]); - - // we will do the factor of 0.5 at the end - btScalar result = m_baseMass * vel[0].dot(vel[0]); - result += omega[0].dot(m_baseInertia * omega[0]); - - for (int i = 0; i < num_links; ++i) { - result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]); - result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]); - } - - return 0.5f * result; -} - -btVector3 btMultiBody::getAngularMomentum() const -{ - int num_links = getNumLinks(); - // TODO: would be better not to allocate memory here - btAlignedObjectArray omega;omega.resize(num_links+1); - btAlignedObjectArray vel;vel.resize(num_links+1); - btAlignedObjectArray rot_from_world;rot_from_world.resize(num_links+1); - compTreeLinkVelocities(&omega[0], &vel[0]); - - rot_from_world[0] = m_baseQuat; - btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0])); - - for (int i = 0; i < num_links; ++i) { - rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1]; - result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1]))); - } - - return result; -} - -void btMultiBody::clearConstraintForces() -{ - m_baseConstraintForce.setValue(0, 0, 0); - m_baseConstraintTorque.setValue(0, 0, 0); - - - for (int i = 0; i < getNumLinks(); ++i) { - m_links[i].m_appliedConstraintForce.setValue(0, 0, 0); - m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0); - } -} -void btMultiBody::clearForcesAndTorques() -{ - m_baseForce.setValue(0, 0, 0); - m_baseTorque.setValue(0, 0, 0); - - - for (int i = 0; i < getNumLinks(); ++i) { - m_links[i].m_appliedForce.setValue(0, 0, 0); - m_links[i].m_appliedTorque.setValue(0, 0, 0); - m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f; - } -} - -void btMultiBody::clearVelocities() -{ - for (int i = 0; i < 6 + getNumLinks(); ++i) - { - m_realBuf[i] = 0.f; - } -} -void btMultiBody::addLinkForce(int i, const btVector3 &f) -{ - m_links[i].m_appliedForce += f; -} - -void btMultiBody::addLinkTorque(int i, const btVector3 &t) -{ - m_links[i].m_appliedTorque += t; -} - -void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f) -{ - m_links[i].m_appliedConstraintForce += f; -} - -void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t) -{ - m_links[i].m_appliedConstraintTorque += t; -} - - - -void btMultiBody::addJointTorque(int i, btScalar Q) -{ - m_links[i].m_jointTorque[0] += Q; -} - -void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q) -{ - m_links[i].m_jointTorque[dof] += Q; -} - -void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q) -{ - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - m_links[i].m_jointTorque[dof] = Q[dof]; -} - -const btVector3 & btMultiBody::getLinkForce(int i) const -{ - return m_links[i].m_appliedForce; -} - -const btVector3 & btMultiBody::getLinkTorque(int i) const -{ - return m_links[i].m_appliedTorque; -} - -btScalar btMultiBody::getJointTorque(int i) const -{ - return m_links[i].m_jointTorque[0]; -} - -btScalar * btMultiBody::getJointTorqueMultiDof(int i) -{ - return &m_links[i].m_jointTorque[0]; -} - -inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross? -{ - btVector3 row0 = btVector3( - v0.x() * v1.x(), - v0.x() * v1.y(), - v0.x() * v1.z()); - btVector3 row1 = btVector3( - v0.y() * v1.x(), - v0.y() * v1.y(), - v0.y() * v1.z()); - btVector3 row2 = btVector3( - v0.z() * v1.x(), - v0.z() * v1.y(), - v0.z() * v1.z()); - - btMatrix3x3 m(row0[0],row0[1],row0[2], - row1[0],row1[1],row1[2], - row2[0],row2[1],row2[2]); - return m; -} - -#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed) -// - -void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m, - bool isConstraintPass) -{ - // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) - // and the base linear & angular accelerations. - - // We apply damping forces in this routine as well as any external forces specified by the - // caller (via addBaseForce etc). - - // output should point to an array of 6 + num_links reals. - // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), - // num_links joint acceleration values. - - // We added support for multi degree of freedom (multi dof) joints. - // In addition we also can compute the joint reaction forces. This is performed in a second pass, - // so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver) - - m_internalNeedsJointFeedback = false; - - int num_links = getNumLinks(); - - const btScalar DAMPING_K1_LINEAR = m_linearDamping; - const btScalar DAMPING_K2_LINEAR = m_linearDamping; - - const btScalar DAMPING_K1_ANGULAR = m_angularDamping; - const btScalar DAMPING_K2_ANGULAR= m_angularDamping; - - btVector3 base_vel = getBaseVel(); - btVector3 base_omega = getBaseOmega(); - - // Temporary matrices/vectors -- use scratch space from caller - // so that we don't have to keep reallocating every frame - - scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount - scratch_v.resize(8*num_links + 6); - scratch_m.resize(4*num_links + 4); - - //btScalar * r_ptr = &scratch_r[0]; - btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results - btVector3 * v_ptr = &scratch_v[0]; - - // vhat_i (top = angular, bottom = linear part) - btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr; - v_ptr += num_links * 2 + 2; - // - // zhat_i^A - btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; - v_ptr += num_links * 2 + 2; - // - // chat_i (note NOT defined for the base) - btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr; - v_ptr += num_links * 2; - // - // Ihat_i^A. - btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1]; - - // Cached 3x3 rotation matrices from parent frame to this frame. - btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; - btMatrix3x3 * rot_from_world = &scratch_m[0]; - - // hhat_i, ahat_i - // hhat is NOT stored for the base (but ahat is) - btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); - btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; - v_ptr += num_links * 2 + 2; - // - // Y_i, invD_i - btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; - btScalar * Y = &scratch_r[0]; - // - //aux variables - static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence) - static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do - static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies - static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) - static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough - static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors - static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child - static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp - static btSpatialTransformationMatrix fromWorld; - fromWorld.m_trnVec.setZero(); - ///////////////// - - // ptr to the joint accel part of the output - btScalar * joint_accel = output + 6; - - // Start of the algorithm proper. - - // First 'upward' loop. - // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. - - rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? - - //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates - spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel); - - if (m_fixedBase) - { - zeroAccSpatFrc[0].setZero(); - } - else - { - btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce; - btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque; - //external forces - zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce)); - - //adding damping terms (only) - btScalar linDampMult = 1., angDampMult = 1.; - zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()), - linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm())); - - // - //p += vhat x Ihat vhat - done in a simpler way - if (m_useGyroTerm) - zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular())); - // - zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear())); - } - - - //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) - spatInertia[0].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0), - // - btMatrix3x3(m_baseMass, 0, 0, - 0, m_baseMass, 0, - 0, 0, m_baseMass), - // - btMatrix3x3(m_baseInertia[0], 0, 0, - 0, m_baseInertia[1], 0, - 0, 0, m_baseInertia[2]) - ); - - rot_from_world[0] = rot_from_parent[0]; - - // - for (int i = 0; i < num_links; ++i) { - const int parent = m_links[i].m_parent; - rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); - rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; - - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - fromWorld.m_rotMat = rot_from_world[i+1]; - fromParent.transform(spatVel[parent+1], spatVel[i+1]); - - // now set vhat_i to its true value by doing - // vhat_i += qidot * shat_i - if(!m_useGlobalVelocities) - { - spatJointVel.setZero(); - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; - - // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint - spatVel[i+1] += spatJointVel; - - // - // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint - //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel; - - } - else - { - fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]); - fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel); - } - - // we can now calculate chat_i - spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]); - - // calculate zhat_i^A - // - //external forces - btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce; - btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; - - zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce )); - -#if 0 - { - - b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f", - i+1, - zeroAccSpatFrc[i+1].m_topVec[0], - zeroAccSpatFrc[i+1].m_topVec[1], - zeroAccSpatFrc[i+1].m_topVec[2], - - zeroAccSpatFrc[i+1].m_bottomVec[0], - zeroAccSpatFrc[i+1].m_bottomVec[1], - zeroAccSpatFrc[i+1].m_bottomVec[2]); - } -#endif - // - //adding damping terms (only) - btScalar linDampMult = 1., angDampMult = 1.; - zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()), - linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm())); - - // calculate Ihat_i^A - //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) - spatInertia[i+1].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0), - // - btMatrix3x3(m_links[i].m_mass, 0, 0, - 0, m_links[i].m_mass, 0, - 0, 0, m_links[i].m_mass), - // - btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0, - 0, m_links[i].m_inertiaLocal[1], 0, - 0, 0, m_links[i].m_inertiaLocal[2]) - ); - // - //p += vhat x Ihat vhat - done in a simpler way - if(m_useGyroTerm) - zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular())); - // - zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear())); - //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); - ////clamp parent's omega - //btScalar parOmegaMod = temp.length(); - //btScalar parOmegaModMax = 1000; - //if(parOmegaMod > parOmegaModMax) - // temp *= parOmegaModMax / parOmegaMod; - //zeroAccSpatFrc[i+1].addLinear(temp); - //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); - //temp = spatCoriolisAcc[i].getLinear(); - //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length()); - - - - //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z()); - //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z()); - //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z()); - } - - // 'Downward' loop. - // (part of TreeForwardDynamics in Mirtich.) - for (int i = num_links - 1; i >= 0; --i) - { - const int parent = m_links[i].m_parent; - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; - // - hDof = spatInertia[i+1] * m_links[i].m_axes[dof]; - // - Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) - - spatCoriolisAcc[i].dot(hDof) - ; - } - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - btScalar *D_row = &D[dof * m_links[i].m_dofCount]; - for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) - { - btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; - D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2); - } - } - - btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; - switch(m_links[i].m_jointType) - { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - invDi[0] = 1.0f / D[0]; - break; - } - case btMultibodyLink::eSpherical: - case btMultibodyLink::ePlanar: - { - static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); - static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse(); - - //unroll the loop? - for(int row = 0; row < 3; ++row) - { - for(int col = 0; col < 3; ++col) - { - invDi[row * 3 + col] = invD3x3[row][col]; - } - } - - break; - } - default: - { - - } - } - - //determine h*D^{-1} - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - spatForceVecTemps[dof].setZero(); - - for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) - { - btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; - // - spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof]; - } - } - - dyadTemp = spatInertia[i+1]; - - //determine (h*D^{-1}) * h^{T} - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; - // - dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]); - } - - fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add); - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - invD_times_Y[dof] = 0.f; - - for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) - { - invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; - } - } - - spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i]; - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; - // - spatForceVecTemps[0] += hDof * invD_times_Y[dof]; - } - - fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); - - zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; - } - - - // Second 'upward' loop - // (part of TreeForwardDynamics in Mirtich) - - if (m_fixedBase) - { - spatAcc[0].setZero(); - } - else - { - if (num_links > 0) - { - m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat; - m_cachedInertiaTopRight = spatInertia[0].m_topRightMat; - m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat; - m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose(); - - } - - solveImatrix(zeroAccSpatFrc[0], result); - spatAcc[0] = -result; - } - - - // now do the loop over the m_links - for (int i = 0; i < num_links; ++i) - { - // qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar) - // a = apar + cor + Sqdd - //or - // qdd = D^{-1} * (Y - h^{T}*(apar+cor)) - // a = apar + Sqdd - - const int parent = m_links[i].m_parent; - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - - fromParent.transform(spatAcc[parent+1], spatAcc[i+1]); - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; - // - Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); - } - - btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; - //D^{-1} * (Y - h^{T}*apar) - mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); - - spatAcc[i+1] += spatCoriolisAcc[i]; - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; - - if (m_links[i].m_jointFeedback) - { - m_internalNeedsJointFeedback = true; - - btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; - btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; - - if (gJointFeedbackInJointFrame) - { - //shift the reaction forces to the joint frame - //linear (force) component is the same - //shift the angular (torque, moment) component using the relative position, m_links[i].m_dVector - angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector); - } - - - if (gJointFeedbackInWorldSpace) - { - if (isConstraintPass) - { - m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec; - } else - { - m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec; - } - } else - { - if (isConstraintPass) - { - m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec; - - } - else - { - m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec; - m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec; - } - } - } - - } - - // transform base accelerations back to the world frame. - btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); - output[0] = omegadot_out[0]; - output[1] = omegadot_out[1]; - output[2] = omegadot_out[2]; - - btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear())); - output[3] = vdot_out[0]; - output[4] = vdot_out[1]; - output[5] = vdot_out[2]; - - ///////////////// - //printf("q = ["); - //printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z()); - //for(int link = 0; link < getNumLinks(); ++link) - // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) - // printf("%.6f ", m_links[link].m_jointPos[dof]); - //printf("]\n"); - //// - //printf("qd = ["); - //for(int dof = 0; dof < getNumDofs() + 6; ++dof) - // printf("%.6f ", m_realBuf[dof]); - //printf("]\n"); - //printf("qdd = ["); - //for(int dof = 0; dof < getNumDofs() + 6; ++dof) - // printf("%.6f ", output[dof]); - //printf("]\n"); - ///////////////// - - // Final step: add the accelerations (times dt) to the velocities. - - if (!isConstraintPass) - { - if(dt > 0.) - applyDeltaVeeMultiDof(output, dt); - - } - ///// - //btScalar angularThres = 1; - //btScalar maxAngVel = 0.; - //bool scaleDown = 1.; - //for(int link = 0; link < m_links.size(); ++link) - //{ - // if(spatVel[link+1].getAngular().length() > maxAngVel) - // { - // maxAngVel = spatVel[link+1].getAngular().length(); - // scaleDown = angularThres / spatVel[link+1].getAngular().length(); - // break; - // } - //} - - //if(scaleDown != 1.) - //{ - // for(int link = 0; link < m_links.size(); ++link) - // { - // if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical) - // { - // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) - // getJointVelMultiDof(link)[dof] *= scaleDown; - // } - // } - //} - ///// - - ///////////////////// - if(m_useGlobalVelocities) - { - for (int i = 0; i < num_links; ++i) - { - const int parent = m_links[i].m_parent; - //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done - //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done - - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - fromWorld.m_rotMat = rot_from_world[i+1]; - - // vhat_i = i_xhat_p(i) * vhat_p(i) - fromParent.transform(spatVel[parent+1], spatVel[i+1]); - //nice alternative below (using operator *) but it generates temps - ///////////////////////////////////////////////////////////// - - // now set vhat_i to its true value by doing - // vhat_i += qidot * shat_i - spatJointVel.setZero(); - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; - - // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint - spatVel[i+1] += spatJointVel; - - - fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity); - fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity); - } - } - -} - - - -void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const -{ - int num_links = getNumLinks(); - ///solve I * x = rhs, so the result = invI * rhs - if (num_links == 0) - { - // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier - result[0] = rhs_bot[0] / m_baseInertia[0]; - result[1] = rhs_bot[1] / m_baseInertia[1]; - result[2] = rhs_bot[2] / m_baseInertia[2]; - result[3] = rhs_top[0] / m_baseMass; - result[4] = rhs_top[1] / m_baseMass; - result[5] = rhs_top[2] / m_baseMass; - } else - { - /// Special routine for calculating the inverse of a spatial inertia matrix - ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices - btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f; - btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv; - btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse(); - tmp = invIupper_right * m_cachedInertiaLowerRight; - btMatrix3x3 invI_upper_left = (tmp * Binv); - btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); - tmp = m_cachedInertiaTopLeft * invI_upper_left; - tmp[0][0]-= 1.0; - tmp[1][1]-= 1.0; - tmp[2][2]-= 1.0; - btMatrix3x3 invI_lower_left = (Binv * tmp); - - //multiply result = invI * rhs - { - btVector3 vtop = invI_upper_left*rhs_top; - btVector3 tmp; - tmp = invIupper_right * rhs_bot; - vtop += tmp; - btVector3 vbot = invI_lower_left*rhs_top; - tmp = invI_lower_right * rhs_bot; - vbot += tmp; - result[0] = vtop[0]; - result[1] = vtop[1]; - result[2] = vtop[2]; - result[3] = vbot[0]; - result[4] = vbot[1]; - result[5] = vbot[2]; - } - - } -} -void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const -{ - int num_links = getNumLinks(); - ///solve I * x = rhs, so the result = invI * rhs - if (num_links == 0) - { - // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier - result.setAngular(rhs.getAngular() / m_baseInertia); - result.setLinear(rhs.getLinear() / m_baseMass); - } else - { - /// Special routine for calculating the inverse of a spatial inertia matrix - ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices - btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f; - btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv; - btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse(); - tmp = invIupper_right * m_cachedInertiaLowerRight; - btMatrix3x3 invI_upper_left = (tmp * Binv); - btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); - tmp = m_cachedInertiaTopLeft * invI_upper_left; - tmp[0][0]-= 1.0; - tmp[1][1]-= 1.0; - tmp[2][2]-= 1.0; - btMatrix3x3 invI_lower_left = (Binv * tmp); - - //multiply result = invI * rhs - { - btVector3 vtop = invI_upper_left*rhs.getLinear(); - btVector3 tmp; - tmp = invIupper_right * rhs.getAngular(); - vtop += tmp; - btVector3 vbot = invI_lower_left*rhs.getLinear(); - tmp = invI_lower_right * rhs.getAngular(); - vbot += tmp; - result.setVector(vtop, vbot); - } - - } -} - -void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const -{ - for (int row = 0; row < rowsA; row++) - { - for (int col = 0; col < colsB; col++) - { - pC[row * colsB + col] = 0.f; - for (int inner = 0; inner < rowsB; inner++) - { - pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB]; - } - } - } -} - -void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, - btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v) const -{ - // Temporary matrices/vectors -- use scratch space from caller - // so that we don't have to keep reallocating every frame - - - int num_links = getNumLinks(); - scratch_r.resize(m_dofCount); - scratch_v.resize(4*num_links + 4); - - btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0; - btVector3 * v_ptr = &scratch_v[0]; - - // zhat_i^A (scratch space) - btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; - v_ptr += num_links * 2 + 2; - - // rot_from_parent (cached from calcAccelerations) - const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; - - // hhat (cached), accel (scratch) - // hhat is NOT stored for the base (but ahat is) - const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); - btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; - v_ptr += num_links * 2 + 2; - - // Y_i (scratch), invD_i (cached) - const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; - btScalar * Y = r_ptr; - //////////////// - //aux variables - static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies - static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) - static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough - static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors - static btSpatialTransformationMatrix fromParent; - ///////////////// - - // First 'upward' loop. - // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. - - // Fill in zero_acc - // -- set to force/torque on the base, zero otherwise - if (m_fixedBase) - { - zeroAccSpatFrc[0].setZero(); - } else - { - //test forces - fromParent.m_rotMat = rot_from_parent[0]; - fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]); - } - for (int i = 0; i < num_links; ++i) - { - zeroAccSpatFrc[i+1].setZero(); - } - - // 'Downward' loop. - // (part of TreeForwardDynamics in Mirtich.) - for (int i = num_links - 1; i >= 0; --i) - { - const int parent = m_links[i].m_parent; - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] - - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) - ; - } - - btVector3 in_top, in_bottom, out_top, out_bottom; - const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - invD_times_Y[dof] = 0.f; - - for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) - { - invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; - } - } - - // Zp += pXi * (Zi + hi*Yi/Di) - spatForceVecTemps[0] = zeroAccSpatFrc[i+1]; - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; - // - spatForceVecTemps[0] += hDof * invD_times_Y[dof]; - } - - - fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); - - zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; - } - - // ptr to the joint accel part of the output - btScalar * joint_accel = output + 6; - - - // Second 'upward' loop - // (part of TreeForwardDynamics in Mirtich) - - if (m_fixedBase) - { - spatAcc[0].setZero(); - } - else - { - solveImatrix(zeroAccSpatFrc[0], result); - spatAcc[0] = -result; - - } - - // now do the loop over the m_links - for (int i = 0; i < num_links; ++i) - { - const int parent = m_links[i].m_parent; - fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - - fromParent.transform(spatAcc[parent+1], spatAcc[i+1]); - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - { - const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; - // - Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); - } - - const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; - mulMatrix(const_cast(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); - - for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; - } - - // transform base accelerations back to the world frame. - btVector3 omegadot_out; - omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); - output[0] = omegadot_out[0]; - output[1] = omegadot_out[1]; - output[2] = omegadot_out[2]; - - btVector3 vdot_out; - vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear(); - output[3] = vdot_out[0]; - output[4] = vdot_out[1]; - output[5] = vdot_out[2]; - - ///////////////// - //printf("delta = ["); - //for(int dof = 0; dof < getNumDofs() + 6; ++dof) - // printf("%.2f ", output[dof]); - //printf("]\n"); - ///////////////// -} - - - - -void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) -{ - int num_links = getNumLinks(); - // step position by adding dt * velocity - //btVector3 v = getBaseVel(); - //m_basePos += dt * v; - // - btScalar *pBasePos = (pq ? &pq[4] : m_basePos); - btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - // - pBasePos[0] += dt * pBaseVel[0]; - pBasePos[1] += dt * pBaseVel[1]; - pBasePos[2] += dt * pBaseVel[2]; - - /////////////////////////////// - //local functor for quaternion integration (to avoid error prone redundancy) - struct - { - //"exponential map" based on btTransformUtil::integrateTransform(..) - void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) - { - //baseBody => quat is alias and omega is global coor - //!baseBody => quat is alibi and omega is local coor - - btVector3 axis; - btVector3 angvel; - - if(!baseBody) - angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok - else - angvel = omega; - - btScalar fAngle = angvel.length(); - //limit the angular motion - if (fAngle * dt > ANGULAR_MOTION_THRESHOLD) - { - fAngle = btScalar(0.5)*SIMD_HALF_PI / dt; - } - - if ( fAngle < btScalar(0.001) ) - { - // use Taylor's expansions of sync function - axis = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle ); - } - else - { - // sync(fAngle) = sin(c*fAngle)/t - axis = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle ); - } - - if(!baseBody) - quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat; - else - quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) )); - //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse(); - - quat.normalize(); - } - } pQuatUpdateFun; - /////////////////////////////// - - //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); - // - btScalar *pBaseQuat = pq ? pq : m_baseQuat; - btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) - // - static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); - static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); - pQuatUpdateFun(baseOmega, baseQuat, true, dt); - pBaseQuat[0] = baseQuat.x(); - pBaseQuat[1] = baseQuat.y(); - pBaseQuat[2] = baseQuat.z(); - pBaseQuat[3] = baseQuat.w(); - - - //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); - //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); - //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); - - if(pq) - pq += 7; - if(pqd) - pqd += 6; - - // Finally we can update m_jointPos for each of the m_links - for (int i = 0; i < num_links; ++i) - { - btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]); - btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); - - switch(m_links[i].m_jointType) - { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - btScalar jointVel = pJointVel[0]; - pJointPos[0] += dt * jointVel; - break; - } - case btMultibodyLink::eSpherical: - { - static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); - static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); - pQuatUpdateFun(jointVel, jointOri, false, dt); - pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w(); - break; - } - case btMultibodyLink::ePlanar: - { - pJointPos[0] += dt * getJointVelMultiDof(i)[0]; - - btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); - btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); - pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; - pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; - - break; - } - default: - { - } - - } - - m_links[i].updateCacheMultiDof(pq); - - if(pq) - pq += m_links[i].m_posVarCount; - if(pqd) - pqd += m_links[i].m_dofCount; - } -} - -void btMultiBody::fillConstraintJacobianMultiDof(int link, - const btVector3 &contact_point, - const btVector3 &normal_ang, - const btVector3 &normal_lin, - btScalar *jac, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) const -{ - // temporary space - int num_links = getNumLinks(); - int m_dofCount = getNumDofs(); - scratch_v.resize(3*num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang - scratch_m.resize(num_links + 1); - - btVector3 * v_ptr = &scratch_v[0]; - btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1; - btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1; - btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1; - btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); - - scratch_r.resize(m_dofCount); - btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0; - - btMatrix3x3 * rot_from_world = &scratch_m[0]; - - const btVector3 p_minus_com_world = contact_point - m_basePos; - const btVector3 &normal_lin_world = normal_lin; //convenience - const btVector3 &normal_ang_world = normal_ang; - - rot_from_world[0] = btMatrix3x3(m_baseQuat); - - // omega coeffients first. - btVector3 omega_coeffs_world; - omega_coeffs_world = p_minus_com_world.cross(normal_lin_world); - jac[0] = omega_coeffs_world[0] + normal_ang_world[0]; - jac[1] = omega_coeffs_world[1] + normal_ang_world[1]; - jac[2] = omega_coeffs_world[2] + normal_ang_world[2]; - // then v coefficients - jac[3] = normal_lin_world[0]; - jac[4] = normal_lin_world[1]; - jac[5] = normal_lin_world[2]; - - //create link-local versions of p_minus_com and normal - p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world; - n_local_lin[0] = rot_from_world[0] * normal_lin_world; - n_local_ang[0] = rot_from_world[0] * normal_ang_world; - - // Set remaining jac values to zero for now. - for (int i = 6; i < 6 + m_dofCount; ++i) - { - jac[i] = 0; - } - - // Qdot coefficients, if necessary. - if (num_links > 0 && link > -1) { - - // TODO: speed this up -- don't calculate for m_links we don't need. - // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions, - // which is resulting in repeated work being done...) - - // calculate required normals & positions in the local frames. - for (int i = 0; i < num_links; ++i) { - - // transform to local frame - const int parent = m_links[i].m_parent; - const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis); - rot_from_world[i+1] = mtx * rot_from_world[parent+1]; - - n_local_lin[i+1] = mtx * n_local_lin[parent+1]; - n_local_ang[i+1] = mtx * n_local_ang[parent+1]; - p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector; - - // calculate the jacobian entry - switch(m_links[i].m_jointType) - { - case btMultibodyLink::eRevolute: - { - results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); - results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0)); - break; - } - case btMultibodyLink::ePrismatic: - { - results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0)); - break; - } - case btMultibodyLink::eSpherical: - { - results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); - results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1)); - results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2)); - - results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0)); - results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1)); - results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2)); - - break; - } - case btMultibodyLink::ePlanar: - { - results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0)); - results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1)); - results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2)); - - break; - } - default: - { - } - } - - } - - // Now copy through to output. - //printf("jac[%d] = ", link); - while (link != -1) - { - for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) - { - jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof]; - //printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]); - } - - link = m_links[link].m_parent; - } - //printf("]\n"); - } -} - - -void btMultiBody::wakeUp() -{ - m_awake = true; -} - -void btMultiBody::goToSleep() -{ - m_awake = false; -} - -void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) -{ - extern bool gDisableDeactivation; - if (!m_canSleep || gDisableDeactivation) - { - m_awake = true; - m_sleepTimer = 0; - return; - } - - // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities) - btScalar motion = 0; - { - for (int i = 0; i < 6 + m_dofCount; ++i) - motion += m_realBuf[i] * m_realBuf[i]; - } - - - if (motion < SLEEP_EPSILON) { - m_sleepTimer += timestep; - if (m_sleepTimer > SLEEP_TIMEOUT) { - goToSleep(); - } - } else { - m_sleepTimer = 0; - if (!m_awake) - wakeUp(); - } -} - - -void btMultiBody::forwardKinematics(btAlignedObjectArray& world_to_local,btAlignedObjectArray& local_origin) -{ - - int num_links = getNumLinks(); - - // Cached 3x3 rotation matrices from parent frame to this frame. - btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0]; - - rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? - - for (int i = 0; i < num_links; ++i) - { - rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); - } - - int nLinks = getNumLinks(); - ///base + num m_links - world_to_local.resize(nLinks+1); - local_origin.resize(nLinks+1); - - world_to_local[0] = getWorldToBaseRot(); - local_origin[0] = getBasePos(); - - for (int k=0;k& world_to_local,btAlignedObjectArray& local_origin) -{ - world_to_local.resize(getNumLinks()+1); - local_origin.resize(getNumLinks()+1); - - world_to_local[0] = getWorldToBaseRot(); - local_origin[0] = getBasePos(); - - if (getBaseCollider()) - { - btVector3 posr = local_origin[0]; - // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - - getBaseCollider()->setWorldTransform(tr); - - } - - for (int k=0;km_link; - btAssert(link == m); - - int index = link+1; - - btVector3 posr = local_origin[index]; - // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - - col->setWorldTransform(tr); - } - } -} - -int btMultiBody::calculateSerializeBufferSize() const -{ - int sz = sizeof(btMultiBodyData); - return sz; -} - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const -{ - btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer; - getBaseWorldTransform().serialize(mbd->m_baseWorldTransform); - mbd->m_baseMass = this->getBaseMass(); - getBaseInertia().serialize(mbd->m_baseInertia); - { - char* name = (char*) serializer->findNameForPointer(m_baseName); - mbd->m_baseName = (char*)serializer->getUniquePointer(name); - if (mbd->m_baseName) - { - serializer->serializeName(name); - } - } - mbd->m_numLinks = this->getNumLinks(); - if (mbd->m_numLinks) - { - int sz = sizeof(btMultiBodyLinkData); - int numElem = mbd->m_numLinks; - btChunk* chunk = serializer->allocate(sz,numElem); - btMultiBodyLinkData* memPtr = (btMultiBodyLinkData*)chunk->m_oldPtr; - for (int i=0;im_jointType = getLink(i).m_jointType; - memPtr->m_dofCount = getLink(i).m_dofCount; - memPtr->m_posVarCount = getLink(i).m_posVarCount; - - getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia); - memPtr->m_linkMass = getLink(i).m_mass; - memPtr->m_parentIndex = getLink(i).m_parent; - getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset); - getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset); - getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis); - btAssert(memPtr->m_dofCount<=3); - for (int dof = 0;dofm_jointAxisBottom[dof]); - getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]); - - memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof]; - memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof]; - - } - int numPosVar = getLink(i).m_posVarCount; - for (int posvar = 0; posvar < numPosVar;posvar++) - { - memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar]; - } - - - { - char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName); - memPtr->m_linkName = (char*)serializer->getUniquePointer(name); - if (memPtr->m_linkName) - { - serializer->serializeName(name); - } - } - { - char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName); - memPtr->m_jointName = (char*)serializer->getUniquePointer(name); - if (memPtr->m_jointName) - { - serializer->serializeName(name); - } - } - memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider); - - } - serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]); - } - mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0; - - return btMultiBodyDataName; -} +/* + * PURPOSE: + * Class representing an articulated rigid body. Stores the body's + * current state, allows forces and torques to be set, handles + * timestepping and implements Featherstone's algorithm. + * + * COPYRIGHT: + * Copyright (C) Stephen Thompson, , 2011-2013 + * Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) + * Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements + + 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 "btMultiBody.h" +#include "btMultiBodyLink.h" +#include "btMultiBodyLinkCollider.h" +#include "btMultiBodyJointFeedback.h" +#include "LinearMath/btTransformUtil.h" +#include "LinearMath/btSerializer.h" +//#include "Bullet3Common/b3Logging.h" +// #define INCLUDE_GYRO_TERM + +///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals +bool gJointFeedbackInWorldSpace = false; +bool gJointFeedbackInJointFrame = false; + +namespace { + const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2) + const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds +} + +namespace { + void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame + const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates + const btVector3 &top_in, // top part of input vector + const btVector3 &bottom_in, // bottom part of input vector + btVector3 &top_out, // top part of output vector + btVector3 &bottom_out) // bottom part of output vector + { + top_out = rotation_matrix * top_in; + bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in; + } + +#if 0 + void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix, + const btVector3 &displacement, + const btVector3 &top_in, + const btVector3 &bottom_in, + btVector3 &top_out, + btVector3 &bottom_out) + { + top_out = rotation_matrix.transpose() * top_in; + bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in)); + } + + btScalar SpatialDotProduct(const btVector3 &a_top, + const btVector3 &a_bottom, + const btVector3 &b_top, + const btVector3 &b_bottom) + { + return a_bottom.dot(b_top) + a_top.dot(b_bottom); + } + + void SpatialCrossProduct(const btVector3 &a_top, + const btVector3 &a_bottom, + const btVector3 &b_top, + const btVector3 &b_bottom, + btVector3 &top_out, + btVector3 &bottom_out) + { + top_out = a_top.cross(b_top); + bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom); + } +#endif + +} + + +// +// Implementation of class btMultiBody +// + +btMultiBody::btMultiBody(int n_links, + btScalar mass, + const btVector3 &inertia, + bool fixedBase, + bool canSleep, + bool /*deprecatedUseMultiDof*/) + : + m_baseCollider(0), + m_baseName(0), + m_basePos(0,0,0), + m_baseQuat(0, 0, 0, 1), + m_baseMass(mass), + m_baseInertia(inertia), + + m_fixedBase(fixedBase), + m_awake(true), + m_canSleep(canSleep), + m_sleepTimer(0), + m_userObjectPointer(0), + m_userIndex2(-1), + m_userIndex(-1), + m_companionId(-1), + m_linearDamping(0.04f), + m_angularDamping(0.04f), + m_useGyroTerm(true), + m_maxAppliedImpulse(1000.f), + m_maxCoordinateVelocity(100.f), + m_hasSelfCollision(true), + __posUpdated(false), + m_dofCount(0), + m_posVarCnt(0), + m_useRK4(false), + m_useGlobalVelocities(false), + m_internalNeedsJointFeedback(false) +{ + m_cachedInertiaTopLeft.setValue(0,0,0,0,0,0,0,0,0); + m_cachedInertiaTopRight.setValue(0,0,0,0,0,0,0,0,0); + m_cachedInertiaLowerLeft.setValue(0,0,0,0,0,0,0,0,0); + m_cachedInertiaLowerRight.setValue(0,0,0,0,0,0,0,0,0); + m_cachedInertiaValid=false; + + m_links.resize(n_links); + m_matrixBuf.resize(n_links + 1); + + m_baseForce.setValue(0, 0, 0); + m_baseTorque.setValue(0, 0, 0); + + clearConstraintForces(); + clearForcesAndTorques(); +} + +btMultiBody::~btMultiBody() +{ +} + +void btMultiBody::setupFixed(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/) +{ + + m_links[i].m_mass = mass; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].setAxisTop(0, 0., 0., 0.); + m_links[i].setAxisBottom(0, btVector3(0,0,0)); + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].m_dVector = thisPivotToThisComOffset; + m_links[i].m_eVector = parentComToThisPivotOffset; + + m_links[i].m_jointType = btMultibodyLink::eFixed; + m_links[i].m_dofCount = 0; + m_links[i].m_posVarCount = 0; + + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + + m_links[i].updateCacheMultiDof(); + + updateLinksDofOffsets(); + +} + + +void btMultiBody::setupPrismatic(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &jointAxis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, + bool disableParentCollision) +{ + m_dofCount += 1; + m_posVarCnt += 1; + + m_links[i].m_mass = mass; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].setAxisTop(0, 0., 0., 0.); + m_links[i].setAxisBottom(0, jointAxis); + m_links[i].m_eVector = parentComToThisPivotOffset; + m_links[i].m_dVector = thisPivotToThisComOffset; + m_links[i].m_cachedRotParentToThis = rotParentToThis; + + m_links[i].m_jointType = btMultibodyLink::ePrismatic; + m_links[i].m_dofCount = 1; + m_links[i].m_posVarCount = 1; + m_links[i].m_jointPos[0] = 0.f; + m_links[i].m_jointTorque[0] = 0.f; + + if (disableParentCollision) + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // + + m_links[i].updateCacheMultiDof(); + + updateLinksDofOffsets(); +} + +void btMultiBody::setupRevolute(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &jointAxis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, + bool disableParentCollision) +{ + m_dofCount += 1; + m_posVarCnt += 1; + + m_links[i].m_mass = mass; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].setAxisTop(0, jointAxis); + m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset)); + m_links[i].m_dVector = thisPivotToThisComOffset; + m_links[i].m_eVector = parentComToThisPivotOffset; + + m_links[i].m_jointType = btMultibodyLink::eRevolute; + m_links[i].m_dofCount = 1; + m_links[i].m_posVarCount = 1; + m_links[i].m_jointPos[0] = 0.f; + m_links[i].m_jointTorque[0] = 0.f; + + if (disableParentCollision) + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // + m_links[i].updateCacheMultiDof(); + // + updateLinksDofOffsets(); +} + + + +void btMultiBody::setupSpherical(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, + bool disableParentCollision) +{ + + m_dofCount += 3; + m_posVarCnt += 4; + + m_links[i].m_mass = mass; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].m_dVector = thisPivotToThisComOffset; + m_links[i].m_eVector = parentComToThisPivotOffset; + + m_links[i].m_jointType = btMultibodyLink::eSpherical; + m_links[i].m_dofCount = 3; + m_links[i].m_posVarCount = 4; + m_links[i].setAxisTop(0, 1.f, 0.f, 0.f); + m_links[i].setAxisTop(1, 0.f, 1.f, 0.f); + m_links[i].setAxisTop(2, 0.f, 0.f, 1.f); + m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset)); + m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset)); + m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset)); + m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointPos[3] = 1.f; + m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f; + + + if (disableParentCollision) + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // + m_links[i].updateCacheMultiDof(); + // + updateLinksDofOffsets(); +} + +void btMultiBody::setupPlanar(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &rotationAxis, + const btVector3 &parentComToThisComOffset, + bool disableParentCollision) +{ + + m_dofCount += 3; + m_posVarCnt += 3; + + m_links[i].m_mass = mass; + m_links[i].m_inertiaLocal = inertia; + m_links[i].m_parent = parent; + m_links[i].m_zeroRotParentToThis = rotParentToThis; + m_links[i].m_dVector.setZero(); + m_links[i].m_eVector = parentComToThisComOffset; + + // + btVector3 vecNonParallelToRotAxis(1, 0, 0); + if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999) + vecNonParallelToRotAxis.setValue(0, 1, 0); + // + + m_links[i].m_jointType = btMultibodyLink::ePlanar; + m_links[i].m_dofCount = 3; + m_links[i].m_posVarCount = 3; + btVector3 n=rotationAxis.normalized(); + m_links[i].setAxisTop(0, n[0],n[1],n[2]); + m_links[i].setAxisTop(1,0,0,0); + m_links[i].setAxisTop(2,0,0,0); + m_links[i].setAxisBottom(0,0,0,0); + btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis); + m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]); + cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0)); + m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]); + m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; + m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f; + + if (disableParentCollision) + m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + // + m_links[i].updateCacheMultiDof(); + // + updateLinksDofOffsets(); +} + +void btMultiBody::finalizeMultiDof() +{ + m_deltaV.resize(0); + m_deltaV.resize(6 + m_dofCount); + m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels") + m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) + for (int i=0;i=-1); + btAssert(i=m_links.size())) + { + return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + } + + btVector3 result = local_pos; + while (i != -1) { + // 'result' is in frame i. transform it to frame parent(i) + result += getRVector(i); + result = quatRotate(getParentToLocalRot(i).inverse(),result); + i = getParent(i); + } + + // 'result' is now in the base frame. transform it to world frame + result = quatRotate(getWorldToBaseRot().inverse() ,result); + result += getBasePos(); + + return result; +} + +btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const +{ + btAssert(i>=-1); + btAssert(i=m_links.size())) + { + return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + } + + if (i == -1) { + // world to base + return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos())); + } else { + // find position in parent frame, then transform to current frame + return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i); + } +} + +btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const +{ + btAssert(i>=-1); + btAssert(i=m_links.size())) + { + return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + } + + + btVector3 result = local_dir; + while (i != -1) { + result = quatRotate(getParentToLocalRot(i).inverse() , result); + i = getParent(i); + } + result = quatRotate(getWorldToBaseRot().inverse() , result); + return result; +} + +btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const +{ + btAssert(i>=-1); + btAssert(i=m_links.size())) + { + return btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + } + + if (i == -1) { + return quatRotate(getWorldToBaseRot(), world_dir); + } else { + return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir)); + } +} + +btMatrix3x3 btMultiBody::localFrameToWorld(int i, const btMatrix3x3 &local_frame) const +{ + btMatrix3x3 result = local_frame; + btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0)); + btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1)); + btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2)); + result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]); + return result; +} + +void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const +{ + int num_links = getNumLinks(); + // Calculates the velocities of each link (and the base) in its local frame + omega[0] = quatRotate(m_baseQuat ,getBaseOmega()); + vel[0] = quatRotate(m_baseQuat ,getBaseVel()); + + for (int i = 0; i < num_links; ++i) + { + const int parent = m_links[i].m_parent; + + // transform parent vel into this frame, store in omega[i+1], vel[i+1] + SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector, + omega[parent+1], vel[parent+1], + omega[i+1], vel[i+1]); + + // now add qidot * shat_i + //only supported for revolute/prismatic joints, todo: spherical and planar joints + switch(m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + btVector3 axisTop = m_links[i].getAxisTop(0); + btVector3 axisBottom = m_links[i].getAxisBottom(0); + btScalar jointVel = getJointVel(i); + omega[i+1] += jointVel * axisTop; + vel[i+1] += jointVel * axisBottom; + break; + } + default: + { + } + } + } +} + +btScalar btMultiBody::getKineticEnergy() const +{ + int num_links = getNumLinks(); + // TODO: would be better not to allocate memory here + btAlignedObjectArray omega;omega.resize(num_links+1); + btAlignedObjectArray vel;vel.resize(num_links+1); + compTreeLinkVelocities(&omega[0], &vel[0]); + + // we will do the factor of 0.5 at the end + btScalar result = m_baseMass * vel[0].dot(vel[0]); + result += omega[0].dot(m_baseInertia * omega[0]); + + for (int i = 0; i < num_links; ++i) { + result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]); + result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]); + } + + return 0.5f * result; +} + +btVector3 btMultiBody::getAngularMomentum() const +{ + int num_links = getNumLinks(); + // TODO: would be better not to allocate memory here + btAlignedObjectArray omega;omega.resize(num_links+1); + btAlignedObjectArray vel;vel.resize(num_links+1); + btAlignedObjectArray rot_from_world;rot_from_world.resize(num_links+1); + compTreeLinkVelocities(&omega[0], &vel[0]); + + rot_from_world[0] = m_baseQuat; + btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0])); + + for (int i = 0; i < num_links; ++i) { + rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1]; + result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1]))); + } + + return result; +} + +void btMultiBody::clearConstraintForces() +{ + m_baseConstraintForce.setValue(0, 0, 0); + m_baseConstraintTorque.setValue(0, 0, 0); + + + for (int i = 0; i < getNumLinks(); ++i) { + m_links[i].m_appliedConstraintForce.setValue(0, 0, 0); + m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0); + } +} +void btMultiBody::clearForcesAndTorques() +{ + m_baseForce.setValue(0, 0, 0); + m_baseTorque.setValue(0, 0, 0); + + + for (int i = 0; i < getNumLinks(); ++i) { + m_links[i].m_appliedForce.setValue(0, 0, 0); + m_links[i].m_appliedTorque.setValue(0, 0, 0); + m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f; + } +} + +void btMultiBody::clearVelocities() +{ + for (int i = 0; i < 6 + getNumDofs(); ++i) + { + m_realBuf[i] = 0.f; + } +} +void btMultiBody::addLinkForce(int i, const btVector3 &f) +{ + m_links[i].m_appliedForce += f; +} + +void btMultiBody::addLinkTorque(int i, const btVector3 &t) +{ + m_links[i].m_appliedTorque += t; +} + +void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f) +{ + m_links[i].m_appliedConstraintForce += f; +} + +void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t) +{ + m_links[i].m_appliedConstraintTorque += t; +} + + + +void btMultiBody::addJointTorque(int i, btScalar Q) +{ + m_links[i].m_jointTorque[0] += Q; +} + +void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q) +{ + m_links[i].m_jointTorque[dof] += Q; +} + +void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q) +{ + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + m_links[i].m_jointTorque[dof] = Q[dof]; +} + +const btVector3 & btMultiBody::getLinkForce(int i) const +{ + return m_links[i].m_appliedForce; +} + +const btVector3 & btMultiBody::getLinkTorque(int i) const +{ + return m_links[i].m_appliedTorque; +} + +btScalar btMultiBody::getJointTorque(int i) const +{ + return m_links[i].m_jointTorque[0]; +} + +btScalar * btMultiBody::getJointTorqueMultiDof(int i) +{ + return &m_links[i].m_jointTorque[0]; +} + +inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross? +{ + btVector3 row0 = btVector3( + v0.x() * v1.x(), + v0.x() * v1.y(), + v0.x() * v1.z()); + btVector3 row1 = btVector3( + v0.y() * v1.x(), + v0.y() * v1.y(), + v0.y() * v1.z()); + btVector3 row2 = btVector3( + v0.z() * v1.x(), + v0.z() * v1.y(), + v0.z() * v1.z()); + + btMatrix3x3 m(row0[0],row0[1],row0[2], + row1[0],row1[1],row1[2], + row2[0],row2[1],row2[2]); + return m; +} + +#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed) +// + +void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m, + bool isConstraintPass) +{ + // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot) + // and the base linear & angular accelerations. + + // We apply damping forces in this routine as well as any external forces specified by the + // caller (via addBaseForce etc). + + // output should point to an array of 6 + num_links reals. + // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame), + // num_links joint acceleration values. + + // We added support for multi degree of freedom (multi dof) joints. + // In addition we also can compute the joint reaction forces. This is performed in a second pass, + // so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver) + + m_internalNeedsJointFeedback = false; + + int num_links = getNumLinks(); + + const btScalar DAMPING_K1_LINEAR = m_linearDamping; + const btScalar DAMPING_K2_LINEAR = m_linearDamping; + + const btScalar DAMPING_K1_ANGULAR = m_angularDamping; + const btScalar DAMPING_K2_ANGULAR= m_angularDamping; + + btVector3 base_vel = getBaseVel(); + btVector3 base_omega = getBaseOmega(); + + // Temporary matrices/vectors -- use scratch space from caller + // so that we don't have to keep reallocating every frame + + scratch_r.resize(2*m_dofCount + 7); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount + scratch_v.resize(8*num_links + 6); + scratch_m.resize(4*num_links + 4); + + //btScalar * r_ptr = &scratch_r[0]; + btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results + btVector3 * v_ptr = &scratch_v[0]; + + // vhat_i (top = angular, bottom = linear part) + btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr; + v_ptr += num_links * 2 + 2; + // + // zhat_i^A + btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; + v_ptr += num_links * 2 + 2; + // + // chat_i (note NOT defined for the base) + btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr; + v_ptr += num_links * 2; + // + // Ihat_i^A. + btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1]; + + // Cached 3x3 rotation matrices from parent frame to this frame. + btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; + btMatrix3x3 * rot_from_world = &scratch_m[0]; + + // hhat_i, ahat_i + // hhat is NOT stored for the base (but ahat is) + btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); + btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; + v_ptr += num_links * 2 + 2; + // + // Y_i, invD_i + btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; + btScalar * Y = &scratch_r[0]; + // + //aux variables + btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence) + btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do + btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies + btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) + btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough + btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors + btSpatialTransformationMatrix fromParent; //spatial transform from parent to child + btSymmetricSpatialDyad dyadTemp; //inertia matrix temp + btSpatialTransformationMatrix fromWorld; + fromWorld.m_trnVec.setZero(); + ///////////////// + + // ptr to the joint accel part of the output + btScalar * joint_accel = output + 6; + + // Start of the algorithm proper. + + // First 'upward' loop. + // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. + + rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? + + //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates + spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel); + + if (m_fixedBase) + { + zeroAccSpatFrc[0].setZero(); + } + else + { + btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce; + btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque; + //external forces + zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce)); + + //adding damping terms (only) + btScalar linDampMult = 1., angDampMult = 1.; + zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()), + linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm())); + + // + //p += vhat x Ihat vhat - done in a simpler way + if (m_useGyroTerm) + zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular())); + // + zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear())); + } + + + //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) + spatInertia[0].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0), + // + btMatrix3x3(m_baseMass, 0, 0, + 0, m_baseMass, 0, + 0, 0, m_baseMass), + // + btMatrix3x3(m_baseInertia[0], 0, 0, + 0, m_baseInertia[1], 0, + 0, 0, m_baseInertia[2]) + ); + + rot_from_world[0] = rot_from_parent[0]; + + // + for (int i = 0; i < num_links; ++i) { + const int parent = m_links[i].m_parent; + rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); + rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; + + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + fromWorld.m_rotMat = rot_from_world[i+1]; + fromParent.transform(spatVel[parent+1], spatVel[i+1]); + + // now set vhat_i to its true value by doing + // vhat_i += qidot * shat_i + if(!m_useGlobalVelocities) + { + spatJointVel.setZero(); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; + + // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint + spatVel[i+1] += spatJointVel; + + // + // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint + //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel; + + } + else + { + fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]); + fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel); + } + + // we can now calculate chat_i + spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]); + + // calculate zhat_i^A + // + //external forces + btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce; + btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque; + + zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce )); + +#if 0 + { + + b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f", + i+1, + zeroAccSpatFrc[i+1].m_topVec[0], + zeroAccSpatFrc[i+1].m_topVec[1], + zeroAccSpatFrc[i+1].m_topVec[2], + + zeroAccSpatFrc[i+1].m_bottomVec[0], + zeroAccSpatFrc[i+1].m_bottomVec[1], + zeroAccSpatFrc[i+1].m_bottomVec[2]); + } +#endif + // + //adding damping terms (only) + btScalar linDampMult = 1., angDampMult = 1.; + zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().safeNorm()), + linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().safeNorm())); + + // calculate Ihat_i^A + //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) + spatInertia[i+1].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0), + // + btMatrix3x3(m_links[i].m_mass, 0, 0, + 0, m_links[i].m_mass, 0, + 0, 0, m_links[i].m_mass), + // + btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0, + 0, m_links[i].m_inertiaLocal[1], 0, + 0, 0, m_links[i].m_inertiaLocal[2]) + ); + // + //p += vhat x Ihat vhat - done in a simpler way + if(m_useGyroTerm) + zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular())); + // + zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear())); + //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()); + ////clamp parent's omega + //btScalar parOmegaMod = temp.length(); + //btScalar parOmegaModMax = 1000; + //if(parOmegaMod > parOmegaModMax) + // temp *= parOmegaModMax / parOmegaMod; + //zeroAccSpatFrc[i+1].addLinear(temp); + //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length()); + //temp = spatCoriolisAcc[i].getLinear(); + //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length()); + + + + //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z()); + //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z()); + //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z()); + } + + // 'Downward' loop. + // (part of TreeForwardDynamics in Mirtich.) + for (int i = num_links - 1; i >= 0; --i) + { + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + hDof = spatInertia[i+1] * m_links[i].m_axes[dof]; + // + Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] + - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) + - spatCoriolisAcc[i].dot(hDof) + ; + } + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btScalar *D_row = &D[dof * m_links[i].m_dofCount]; + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; + D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2); + } + } + + btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + switch(m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + if (D[0]>=SIMD_EPSILON) + { + invDi[0] = 1.0f / D[0]; + } else + { + invDi[0] = 0; + } + break; + } + case btMultibodyLink::eSpherical: + case btMultibodyLink::ePlanar: + { + btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]); + btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse(); + + //unroll the loop? + for(int row = 0; row < 3; ++row) + { + for(int col = 0; col < 3; ++col) + { + invDi[row * 3 + col] = invD3x3[row][col]; + } + } + + break; + } + default: + { + + } + } + + //determine h*D^{-1} + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + spatForceVecTemps[dof].setZero(); + + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2]; + // + spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof]; + } + } + + dyadTemp = spatInertia[i+1]; + + //determine (h*D^{-1}) * h^{T} + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]); + } + + fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + invD_times_Y[dof] = 0.f; + + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; + } + } + + spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + spatForceVecTemps[0] += hDof * invD_times_Y[dof]; + } + + fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); + + zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; + } + + + // Second 'upward' loop + // (part of TreeForwardDynamics in Mirtich) + + if (m_fixedBase) + { + spatAcc[0].setZero(); + } + else + { + if (num_links > 0) + { + m_cachedInertiaValid = true; + m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat; + m_cachedInertiaTopRight = spatInertia[0].m_topRightMat; + m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat; + m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose(); + + } + + solveImatrix(zeroAccSpatFrc[0], result); + spatAcc[0] = -result; + } + + + // now do the loop over the m_links + for (int i = 0; i < num_links; ++i) + { + // qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar) + // a = apar + cor + Sqdd + //or + // qdd = D^{-1} * (Y - h^{T}*(apar+cor)) + // a = apar + Sqdd + + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + + fromParent.transform(spatAcc[parent+1], spatAcc[i+1]); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); + } + + btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + //D^{-1} * (Y - h^{T}*apar) + mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); + + spatAcc[i+1] += spatCoriolisAcc[i]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; + + if (m_links[i].m_jointFeedback) + { + m_internalNeedsJointFeedback = true; + + btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec; + btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec; + + if (gJointFeedbackInJointFrame) + { + //shift the reaction forces to the joint frame + //linear (force) component is the same + //shift the angular (torque, moment) component using the relative position, m_links[i].m_dVector + angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector); + } + + + if (gJointFeedbackInWorldSpace) + { + if (isConstraintPass) + { + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec; + } else + { + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec; + } + } else + { + if (isConstraintPass) + { + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec; + + } + else + { + m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec; + m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec; + } + } + } + + } + + // transform base accelerations back to the world frame. + btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); + output[0] = omegadot_out[0]; + output[1] = omegadot_out[1]; + output[2] = omegadot_out[2]; + + btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear())); + output[3] = vdot_out[0]; + output[4] = vdot_out[1]; + output[5] = vdot_out[2]; + + ///////////////// + //printf("q = ["); + //printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z()); + //for(int link = 0; link < getNumLinks(); ++link) + // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) + // printf("%.6f ", m_links[link].m_jointPos[dof]); + //printf("]\n"); + //// + //printf("qd = ["); + //for(int dof = 0; dof < getNumDofs() + 6; ++dof) + // printf("%.6f ", m_realBuf[dof]); + //printf("]\n"); + //printf("qdd = ["); + //for(int dof = 0; dof < getNumDofs() + 6; ++dof) + // printf("%.6f ", output[dof]); + //printf("]\n"); + ///////////////// + + // Final step: add the accelerations (times dt) to the velocities. + + if (!isConstraintPass) + { + if(dt > 0.) + applyDeltaVeeMultiDof(output, dt); + + } + ///// + //btScalar angularThres = 1; + //btScalar maxAngVel = 0.; + //bool scaleDown = 1.; + //for(int link = 0; link < m_links.size(); ++link) + //{ + // if(spatVel[link+1].getAngular().length() > maxAngVel) + // { + // maxAngVel = spatVel[link+1].getAngular().length(); + // scaleDown = angularThres / spatVel[link+1].getAngular().length(); + // break; + // } + //} + + //if(scaleDown != 1.) + //{ + // for(int link = 0; link < m_links.size(); ++link) + // { + // if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical) + // { + // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) + // getJointVelMultiDof(link)[dof] *= scaleDown; + // } + // } + //} + ///// + + ///////////////////// + if(m_useGlobalVelocities) + { + for (int i = 0; i < num_links; ++i) + { + const int parent = m_links[i].m_parent; + //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done + //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done + + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + fromWorld.m_rotMat = rot_from_world[i+1]; + + // vhat_i = i_xhat_p(i) * vhat_p(i) + fromParent.transform(spatVel[parent+1], spatVel[i+1]); + //nice alternative below (using operator *) but it generates temps + ///////////////////////////////////////////////////////////// + + // now set vhat_i to its true value by doing + // vhat_i += qidot * shat_i + spatJointVel.setZero(); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; + + // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint + spatVel[i+1] += spatJointVel; + + + fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity); + fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity); + } + } + +} + + + +void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, btScalar result[6]) const +{ + int num_links = getNumLinks(); + ///solve I * x = rhs, so the result = invI * rhs + if (num_links == 0) + { + // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier + + if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON)) + { + result[0] = rhs_bot[0] / m_baseInertia[0]; + result[1] = rhs_bot[1] / m_baseInertia[1]; + result[2] = rhs_bot[2] / m_baseInertia[2]; + } else + { + result[0] = 0; + result[1] = 0; + result[2] = 0; + } + if (m_baseMass>=SIMD_EPSILON) + { + result[3] = rhs_top[0] / m_baseMass; + result[4] = rhs_top[1] / m_baseMass; + result[5] = rhs_top[2] / m_baseMass; + } else + { + result[3] = 0; + result[4] = 0; + result[5] = 0; + } + } else + { + if (!m_cachedInertiaValid) + { + for (int i=0;i<6;i++) + { + result[i] = 0.f; + } + return; + } + /// Special routine for calculating the inverse of a spatial inertia matrix + ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices + btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f; + btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv; + btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse(); + tmp = invIupper_right * m_cachedInertiaLowerRight; + btMatrix3x3 invI_upper_left = (tmp * Binv); + btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); + tmp = m_cachedInertiaTopLeft * invI_upper_left; + tmp[0][0]-= 1.0; + tmp[1][1]-= 1.0; + tmp[2][2]-= 1.0; + btMatrix3x3 invI_lower_left = (Binv * tmp); + + //multiply result = invI * rhs + { + btVector3 vtop = invI_upper_left*rhs_top; + btVector3 tmp; + tmp = invIupper_right * rhs_bot; + vtop += tmp; + btVector3 vbot = invI_lower_left*rhs_top; + tmp = invI_lower_right * rhs_bot; + vbot += tmp; + result[0] = vtop[0]; + result[1] = vtop[1]; + result[2] = vtop[2]; + result[3] = vbot[0]; + result[4] = vbot[1]; + result[5] = vbot[2]; + } + + } +} +void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const +{ + int num_links = getNumLinks(); + ///solve I * x = rhs, so the result = invI * rhs + if (num_links == 0) + { + // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier + if ((m_baseInertia[0] >= SIMD_EPSILON) && (m_baseInertia[1] >= SIMD_EPSILON) && (m_baseInertia[2] >= SIMD_EPSILON)) + { + result.setAngular(rhs.getAngular() / m_baseInertia); + } else + { + result.setAngular(btVector3(0,0,0)); + } + if (m_baseMass>=SIMD_EPSILON) + { + result.setLinear(rhs.getLinear() / m_baseMass); + } else + { + result.setLinear(btVector3(0,0,0)); + } + } else + { + /// Special routine for calculating the inverse of a spatial inertia matrix + ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices + if (!m_cachedInertiaValid) + { + result.setLinear(btVector3(0,0,0)); + result.setAngular(btVector3(0,0,0)); + result.setVector(btVector3(0,0,0),btVector3(0,0,0)); + return; + } + btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f; + btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv; + btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse(); + tmp = invIupper_right * m_cachedInertiaLowerRight; + btMatrix3x3 invI_upper_left = (tmp * Binv); + btMatrix3x3 invI_lower_right = (invI_upper_left).transpose(); + tmp = m_cachedInertiaTopLeft * invI_upper_left; + tmp[0][0]-= 1.0; + tmp[1][1]-= 1.0; + tmp[2][2]-= 1.0; + btMatrix3x3 invI_lower_left = (Binv * tmp); + + //multiply result = invI * rhs + { + btVector3 vtop = invI_upper_left*rhs.getLinear(); + btVector3 tmp; + tmp = invIupper_right * rhs.getAngular(); + vtop += tmp; + btVector3 vbot = invI_lower_left*rhs.getLinear(); + tmp = invI_lower_right * rhs.getAngular(); + vbot += tmp; + result.setVector(vtop, vbot); + } + + } +} + +void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const +{ + for (int row = 0; row < rowsA; row++) + { + for (int col = 0; col < colsB; col++) + { + pC[row * colsB + col] = 0.f; + for (int inner = 0; inner < rowsB; inner++) + { + pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB]; + } + } + } +} + +void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, + btAlignedObjectArray &scratch_r, btAlignedObjectArray &scratch_v) const +{ + // Temporary matrices/vectors -- use scratch space from caller + // so that we don't have to keep reallocating every frame + + + int num_links = getNumLinks(); + scratch_r.resize(m_dofCount); + scratch_v.resize(4*num_links + 4); + + btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0; + btVector3 * v_ptr = &scratch_v[0]; + + // zhat_i^A (scratch space) + btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr; + v_ptr += num_links * 2 + 2; + + // rot_from_parent (cached from calcAccelerations) + const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0]; + + // hhat (cached), accel (scratch) + // hhat is NOT stored for the base (but ahat is) + const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); + btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; + v_ptr += num_links * 2 + 2; + + // Y_i (scratch), invD_i (cached) + const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; + btScalar * Y = r_ptr; + //////////////// + //aux variables + btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies + btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel) + btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough + btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors + btSpatialTransformationMatrix fromParent; + ///////////////// + + // First 'upward' loop. + // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich. + + // Fill in zero_acc + // -- set to force/torque on the base, zero otherwise + if (m_fixedBase) + { + zeroAccSpatFrc[0].setZero(); + } else + { + //test forces + fromParent.m_rotMat = rot_from_parent[0]; + fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]); + } + for (int i = 0; i < num_links; ++i) + { + zeroAccSpatFrc[i+1].setZero(); + } + + // 'Downward' loop. + // (part of TreeForwardDynamics in Mirtich.) + for (int i = num_links - 1; i >= 0; --i) + { + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] + - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) + ; + } + + btVector3 in_top, in_bottom, out_top, out_bottom; + const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + invD_times_Y[dof] = 0.f; + + for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) + { + invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2]; + } + } + + // Zp += pXi * (Zi + hi*Yi/Di) + spatForceVecTemps[0] = zeroAccSpatFrc[i+1]; + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + spatForceVecTemps[0] += hDof * invD_times_Y[dof]; + } + + + fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]); + + zeroAccSpatFrc[parent+1] += spatForceVecTemps[1]; + } + + // ptr to the joint accel part of the output + btScalar * joint_accel = output + 6; + + + // Second 'upward' loop + // (part of TreeForwardDynamics in Mirtich) + + if (m_fixedBase) + { + spatAcc[0].setZero(); + } + else + { + solveImatrix(zeroAccSpatFrc[0], result); + spatAcc[0] = -result; + + } + + // now do the loop over the m_links + for (int i = 0; i < num_links; ++i) + { + const int parent = m_links[i].m_parent; + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + + fromParent.transform(spatAcc[parent+1], spatAcc[i+1]); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + { + const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof]; + // + Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof); + } + + const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset]; + mulMatrix(const_cast(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]); + + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof]; + } + + // transform base accelerations back to the world frame. + btVector3 omegadot_out; + omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular(); + output[0] = omegadot_out[0]; + output[1] = omegadot_out[1]; + output[2] = omegadot_out[2]; + + btVector3 vdot_out; + vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear(); + output[3] = vdot_out[0]; + output[4] = vdot_out[1]; + output[5] = vdot_out[2]; + + ///////////////// + //printf("delta = ["); + //for(int dof = 0; dof < getNumDofs() + 6; ++dof) + // printf("%.2f ", output[dof]); + //printf("]\n"); + ///////////////// +} + + + + +void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) +{ + int num_links = getNumLinks(); + // step position by adding dt * velocity + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + btScalar *pBasePos = (pq ? &pq[4] : m_basePos); + btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) + // + pBasePos[0] += dt * pBaseVel[0]; + pBasePos[1] += dt * pBaseVel[1]; + pBasePos[2] += dt * pBaseVel[2]; + + /////////////////////////////// + //local functor for quaternion integration (to avoid error prone redundancy) + struct + { + //"exponential map" based on btTransformUtil::integrateTransform(..) + void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) + { + //baseBody => quat is alias and omega is global coor + //!baseBody => quat is alibi and omega is local coor + + btVector3 axis; + btVector3 angvel; + + if(!baseBody) + angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok + else + angvel = omega; + + btScalar fAngle = angvel.length(); + //limit the angular motion + if (fAngle * dt > ANGULAR_MOTION_THRESHOLD) + { + fAngle = btScalar(0.5)*SIMD_HALF_PI / dt; + } + + if ( fAngle < btScalar(0.001) ) + { + // use Taylor's expansions of sync function + axis = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle ); + } + else + { + // sync(fAngle) = sin(c*fAngle)/t + axis = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle ); + } + + if(!baseBody) + quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat; + else + quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) )); + //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse(); + + quat.normalize(); + } + } pQuatUpdateFun; + /////////////////////////////// + + //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); + // + btScalar *pBaseQuat = pq ? pq : m_baseQuat; + btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) + // + btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + pQuatUpdateFun(baseOmega, baseQuat, true, dt); + pBaseQuat[0] = baseQuat.x(); + pBaseQuat[1] = baseQuat.y(); + pBaseQuat[2] = baseQuat.z(); + pBaseQuat[3] = baseQuat.w(); + + + //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z()); + //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z()); + //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w()); + + if(pq) + pq += 7; + if(pqd) + pqd += 6; + + // Finally we can update m_jointPos for each of the m_links + for (int i = 0; i < num_links; ++i) + { + btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]); + btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); + + switch(m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + btScalar jointVel = pJointVel[0]; + pJointPos[0] += dt * jointVel; + break; + } + case btMultibodyLink::eSpherical: + { + btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); + btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + pQuatUpdateFun(jointVel, jointOri, false, dt); + pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w(); + break; + } + case btMultibodyLink::ePlanar: + { + pJointPos[0] += dt * getJointVelMultiDof(i)[0]; + + btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); + btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); + pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; + pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; + + break; + } + default: + { + } + + } + + m_links[i].updateCacheMultiDof(pq); + + if(pq) + pq += m_links[i].m_posVarCount; + if(pqd) + pqd += m_links[i].m_dofCount; + } +} + +void btMultiBody::fillConstraintJacobianMultiDof(int link, + const btVector3 &contact_point, + const btVector3 &normal_ang, + const btVector3 &normal_lin, + btScalar *jac, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m) const +{ + // temporary space + int num_links = getNumLinks(); + int m_dofCount = getNumDofs(); + scratch_v.resize(3*num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang + scratch_m.resize(num_links + 1); + + btVector3 * v_ptr = &scratch_v[0]; + btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1; + btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1; + btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1; + btAssert(v_ptr - &scratch_v[0] == scratch_v.size()); + + scratch_r.resize(m_dofCount); + btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0; + + btMatrix3x3 * rot_from_world = &scratch_m[0]; + + const btVector3 p_minus_com_world = contact_point - m_basePos; + const btVector3 &normal_lin_world = normal_lin; //convenience + const btVector3 &normal_ang_world = normal_ang; + + rot_from_world[0] = btMatrix3x3(m_baseQuat); + + // omega coeffients first. + btVector3 omega_coeffs_world; + omega_coeffs_world = p_minus_com_world.cross(normal_lin_world); + jac[0] = omega_coeffs_world[0] + normal_ang_world[0]; + jac[1] = omega_coeffs_world[1] + normal_ang_world[1]; + jac[2] = omega_coeffs_world[2] + normal_ang_world[2]; + // then v coefficients + jac[3] = normal_lin_world[0]; + jac[4] = normal_lin_world[1]; + jac[5] = normal_lin_world[2]; + + //create link-local versions of p_minus_com and normal + p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world; + n_local_lin[0] = rot_from_world[0] * normal_lin_world; + n_local_ang[0] = rot_from_world[0] * normal_ang_world; + + // Set remaining jac values to zero for now. + for (int i = 6; i < 6 + m_dofCount; ++i) + { + jac[i] = 0; + } + + // Qdot coefficients, if necessary. + if (num_links > 0 && link > -1) { + + // TODO: speed this up -- don't calculate for m_links we don't need. + // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions, + // which is resulting in repeated work being done...) + + // calculate required normals & positions in the local frames. + for (int i = 0; i < num_links; ++i) { + + // transform to local frame + const int parent = m_links[i].m_parent; + const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis); + rot_from_world[i+1] = mtx * rot_from_world[parent+1]; + + n_local_lin[i+1] = mtx * n_local_lin[parent+1]; + n_local_ang[i+1] = mtx * n_local_ang[parent+1]; + p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector; + + // calculate the jacobian entry + switch(m_links[i].m_jointType) + { + case btMultibodyLink::eRevolute: + { + results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); + results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0)); + break; + } + case btMultibodyLink::ePrismatic: + { + results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0)); + break; + } + case btMultibodyLink::eSpherical: + { + results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0)); + results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1)); + results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2)); + + results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0)); + results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1)); + results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2)); + + break; + } + case btMultibodyLink::ePlanar: + { + results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0)); + results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1)); + results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2)); + + break; + } + default: + { + } + } + + } + + // Now copy through to output. + //printf("jac[%d] = ", link); + while (link != -1) + { + for(int dof = 0; dof < m_links[link].m_dofCount; ++dof) + { + jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof]; + //printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]); + } + + link = m_links[link].m_parent; + } + //printf("]\n"); + } +} + + +void btMultiBody::wakeUp() +{ + m_sleepTimer = 0; + m_awake = true; +} + +void btMultiBody::goToSleep() +{ + m_awake = false; +} + +void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep) +{ + extern bool gDisableDeactivation; + if (!m_canSleep || gDisableDeactivation) + { + m_awake = true; + m_sleepTimer = 0; + return; + } + + // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities) + btScalar motion = 0; + { + for (int i = 0; i < 6 + m_dofCount; ++i) + motion += m_realBuf[i] * m_realBuf[i]; + } + + + if (motion < SLEEP_EPSILON) { + m_sleepTimer += timestep; + if (m_sleepTimer > SLEEP_TIMEOUT) { + goToSleep(); + } + } else { + m_sleepTimer = 0; + if (!m_awake) + wakeUp(); + } +} + + +void btMultiBody::forwardKinematics(btAlignedObjectArray& world_to_local,btAlignedObjectArray& local_origin) +{ + + int num_links = getNumLinks(); + + // Cached 3x3 rotation matrices from parent frame to this frame. + btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0]; + + rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!? + + for (int i = 0; i < num_links; ++i) + { + rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); + } + + int nLinks = getNumLinks(); + ///base + num m_links + world_to_local.resize(nLinks+1); + local_origin.resize(nLinks+1); + + world_to_local[0] = getWorldToBaseRot(); + local_origin[0] = getBasePos(); + + for (int k=0;k& world_to_local,btAlignedObjectArray& local_origin) +{ + world_to_local.resize(getNumLinks()+1); + local_origin.resize(getNumLinks()+1); + + world_to_local[0] = getWorldToBaseRot(); + local_origin[0] = getBasePos(); + + if (getBaseCollider()) + { + btVector3 posr = local_origin[0]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + + getBaseCollider()->setWorldTransform(tr); + + } + + for (int k=0;km_link; + btAssert(link == m); + + int index = link+1; + + btVector3 posr = local_origin[index]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + + col->setWorldTransform(tr); + } + } +} + +int btMultiBody::calculateSerializeBufferSize() const +{ + int sz = sizeof(btMultiBodyData); + return sz; +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const +{ + btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer; + getBasePos().serialize(mbd->m_baseWorldPosition); + getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation); + getBaseVel().serialize(mbd->m_baseLinearVelocity); + getBaseOmega().serialize(mbd->m_baseAngularVelocity); + + mbd->m_baseMass = this->getBaseMass(); + getBaseInertia().serialize(mbd->m_baseInertia); + { + char* name = (char*) serializer->findNameForPointer(m_baseName); + mbd->m_baseName = (char*)serializer->getUniquePointer(name); + if (mbd->m_baseName) + { + serializer->serializeName(name); + } + } + mbd->m_numLinks = this->getNumLinks(); + if (mbd->m_numLinks) + { + int sz = sizeof(btMultiBodyLinkData); + int numElem = mbd->m_numLinks; + btChunk* chunk = serializer->allocate(sz,numElem); + btMultiBodyLinkData* memPtr = (btMultiBodyLinkData*)chunk->m_oldPtr; + for (int i=0;im_jointType = getLink(i).m_jointType; + memPtr->m_dofCount = getLink(i).m_dofCount; + memPtr->m_posVarCount = getLink(i).m_posVarCount; + + getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia); + + getLink(i).m_absFrameTotVelocity.m_topVec.serialize(memPtr->m_absFrameTotVelocityTop); + getLink(i).m_absFrameTotVelocity.m_bottomVec.serialize(memPtr->m_absFrameTotVelocityBottom); + getLink(i).m_absFrameLocVelocity.m_topVec.serialize(memPtr->m_absFrameLocVelocityTop); + getLink(i).m_absFrameLocVelocity.m_bottomVec.serialize(memPtr->m_absFrameLocVelocityBottom); + + memPtr->m_linkMass = getLink(i).m_mass; + memPtr->m_parentIndex = getLink(i).m_parent; + memPtr->m_jointDamping = getLink(i).m_jointDamping; + memPtr->m_jointFriction = getLink(i).m_jointFriction; + memPtr->m_jointLowerLimit = getLink(i).m_jointLowerLimit; + memPtr->m_jointUpperLimit = getLink(i).m_jointUpperLimit; + memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce; + memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity; + + getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset); + getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset); + getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis); + btAssert(memPtr->m_dofCount<=3); + for (int dof = 0;dofm_jointAxisBottom[dof]); + getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]); + + memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof]; + memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof]; + + } + int numPosVar = getLink(i).m_posVarCount; + for (int posvar = 0; posvar < numPosVar;posvar++) + { + memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar]; + } + + + { + char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName); + memPtr->m_linkName = (char*)serializer->getUniquePointer(name); + if (memPtr->m_linkName) + { + serializer->serializeName(name); + } + } + { + char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName); + memPtr->m_jointName = (char*)serializer->getUniquePointer(name); + if (memPtr->m_jointName) + { + serializer->serializeName(name); + } + } + memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider); + + } + serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]); + } + mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0; + + // Fill padding with zeros to appease msan. +#ifdef BT_USE_DOUBLE_PRECISION + memset(mbd->m_padding, 0, sizeof(mbd->m_padding)); +#endif + + return btMultiBodyDataName; +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBody.h similarity index 86% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBody.h index 8fbe6cda827a..23ea5533f817 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBody.h @@ -1,760 +1,836 @@ -/* - * PURPOSE: - * Class representing an articulated rigid body. Stores the body's - * current state, allows forces and torques to be set, handles - * timestepping and implements Featherstone's algorithm. - * - * COPYRIGHT: - * Copyright (C) Stephen Thompson, , 2011-2013 - * Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) - * Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements - - This software is provided 'as-is', without any express or implied warranty. - In no event will the authors be held liable for any damages arising from the use of this software. - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it freely, - subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. - - */ - - -#ifndef BT_MULTIBODY_H -#define BT_MULTIBODY_H - -#include "LinearMath/btScalar.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btQuaternion.h" -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btAlignedObjectArray.h" - - -///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms -#ifdef BT_USE_DOUBLE_PRECISION - #define btMultiBodyData btMultiBodyDoubleData - #define btMultiBodyDataName "btMultiBodyDoubleData" - #define btMultiBodyLinkData btMultiBodyLinkDoubleData - #define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData" -#else - #define btMultiBodyData btMultiBodyFloatData - #define btMultiBodyDataName "btMultiBodyFloatData" - #define btMultiBodyLinkData btMultiBodyLinkFloatData - #define btMultiBodyLinkDataName "btMultiBodyLinkFloatData" -#endif //BT_USE_DOUBLE_PRECISION - -#include "btMultiBodyLink.h" -class btMultiBodyLinkCollider; - -ATTRIBUTE_ALIGNED16(class) btMultiBody -{ -public: - - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - // - // initialization - // - - btMultiBody(int n_links, // NOT including the base - btScalar mass, // mass of base - const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal - bool fixedBase, // whether the base is fixed (true) or can move (false) - bool canSleep, bool deprecatedMultiDof=true); - - - virtual ~btMultiBody(); - - //note: fixed link collision with parent is always disabled - void setupFixed(int linkIndex, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true); - - - void setupPrismatic(int i, - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, - const btVector3 &jointAxis, - const btVector3 &parentComToThisPivotOffset, - const btVector3 &thisPivotToThisComOffset, - bool disableParentCollision); - - void setupRevolute(int linkIndex, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parentIndex, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &jointAxis, // in my frame - const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame - const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame - bool disableParentCollision=false); - - void setupSpherical(int linkIndex, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame - const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame - bool disableParentCollision=false); - - void setupPlanar(int i, // 0 to num_links-1 - btScalar mass, - const btVector3 &inertia, - int parent, - const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 - const btVector3 &rotationAxis, - const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame - bool disableParentCollision=false); - - const btMultibodyLink& getLink(int index) const - { - return m_links[index]; - } - - btMultibodyLink& getLink(int index) - { - return m_links[index]; - } - - - void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base - { - m_baseCollider = collider; - } - const btMultiBodyLinkCollider* getBaseCollider() const - { - return m_baseCollider; - } - btMultiBodyLinkCollider* getBaseCollider() - { - return m_baseCollider; - } - - // - // get parent - // input: link num from 0 to num_links-1 - // output: link num from 0 to num_links-1, OR -1 to mean the base. - // - int getParent(int link_num) const; - - - // - // get number of m_links, masses, moments of inertia - // - - int getNumLinks() const { return m_links.size(); } - int getNumDofs() const { return m_dofCount; } - int getNumPosVars() const { return m_posVarCnt; } - btScalar getBaseMass() const { return m_baseMass; } - const btVector3 & getBaseInertia() const { return m_baseInertia; } - btScalar getLinkMass(int i) const; - const btVector3 & getLinkInertia(int i) const; - - - - // - // change mass (incomplete: can only change base mass and inertia at present) - // - - void setBaseMass(btScalar mass) { m_baseMass = mass; } - void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; } - - - // - // get/set pos/vel/rot/omega for the base link - // - - const btVector3 & getBasePos() const { return m_basePos; } // in world frame - const btVector3 getBaseVel() const - { - return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]); - } // in world frame - const btQuaternion & getWorldToBaseRot() const - { - return m_baseQuat; - } // rotates world vectors into base frame - btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame - - void setBasePos(const btVector3 &pos) - { - m_basePos = pos; - } - - void setBaseWorldTransform(const btTransform& tr) - { - setBasePos(tr.getOrigin()); - setWorldToBaseRot(tr.getRotation().inverse()); - - } - - btTransform getBaseWorldTransform() const - { - btTransform tr; - tr.setOrigin(getBasePos()); - tr.setRotation(getWorldToBaseRot().inverse()); - return tr; - } - - void setBaseVel(const btVector3 &vel) - { - - m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; - } - void setWorldToBaseRot(const btQuaternion &rot) - { - m_baseQuat = rot; //m_baseQuat asumed to ba alias!? - } - void setBaseOmega(const btVector3 &omega) - { - m_realBuf[0]=omega[0]; - m_realBuf[1]=omega[1]; - m_realBuf[2]=omega[2]; - } - - - // - // get/set pos/vel for child m_links (i = 0 to num_links-1) - // - - btScalar getJointPos(int i) const; - btScalar getJointVel(int i) const; - - btScalar * getJointVelMultiDof(int i); - btScalar * getJointPosMultiDof(int i); - - const btScalar * getJointVelMultiDof(int i) const ; - const btScalar * getJointPosMultiDof(int i) const ; - - void setJointPos(int i, btScalar q); - void setJointVel(int i, btScalar qdot); - void setJointPosMultiDof(int i, btScalar *q); - void setJointVelMultiDof(int i, btScalar *qdot); - - - - // - // direct access to velocities as a vector of 6 + num_links elements. - // (omega first, then v, then joint velocities.) - // - const btScalar * getVelocityVector() const - { - return &m_realBuf[0]; - } -/* btScalar * getVelocityVector() - { - return &real_buf[0]; - } - */ - - // - // get the frames of reference (positions and orientations) of the child m_links - // (i = 0 to num_links-1) - // - - const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords - const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. - - - // - // transform vectors in local frame of link i to world frame (or vice versa) - // - btVector3 localPosToWorld(int i, const btVector3 &vec) const; - btVector3 localDirToWorld(int i, const btVector3 &vec) const; - btVector3 worldPosToLocal(int i, const btVector3 &vec) const; - btVector3 worldDirToLocal(int i, const btVector3 &vec) const; - - - // - // calculate kinetic energy and angular momentum - // useful for debugging. - // - - btScalar getKineticEnergy() const; - btVector3 getAngularMomentum() const; - - - // - // set external forces and torques. Note all external forces/torques are given in the WORLD frame. - // - - void clearForcesAndTorques(); - void clearConstraintForces(); - - void clearVelocities(); - - void addBaseForce(const btVector3 &f) - { - m_baseForce += f; - } - void addBaseTorque(const btVector3 &t) { m_baseTorque += t; } - void addLinkForce(int i, const btVector3 &f); - void addLinkTorque(int i, const btVector3 &t); - - void addBaseConstraintForce(const btVector3 &f) - { - m_baseConstraintForce += f; - } - void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; } - void addLinkConstraintForce(int i, const btVector3 &f); - void addLinkConstraintTorque(int i, const btVector3 &t); - - -void addJointTorque(int i, btScalar Q); - void addJointTorqueMultiDof(int i, int dof, btScalar Q); - void addJointTorqueMultiDof(int i, const btScalar *Q); - - const btVector3 & getBaseForce() const { return m_baseForce; } - const btVector3 & getBaseTorque() const { return m_baseTorque; } - const btVector3 & getLinkForce(int i) const; - const btVector3 & getLinkTorque(int i) const; - btScalar getJointTorque(int i) const; - btScalar * getJointTorqueMultiDof(int i); - - - // - // dynamics routines. - // - - // timestep the velocities (given the external forces/torques set using addBaseForce etc). - // also sets up caches for calcAccelerationDeltas. - // - // Note: the caller must provide three vectors which are used as - // temporary scratch space. The idea here is to reduce dynamic - // memory allocation: the same scratch vectors can be re-used - // again and again for different Multibodies, instead of each - // btMultiBody allocating (and then deallocating) their own - // individual scratch buffers. This gives a considerable speed - // improvement, at least on Windows (where dynamic memory - // allocation appears to be fairly slow). - // - - - void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m, - bool isConstraintPass=false - ); - -///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead - void stepVelocitiesMultiDof(btScalar dt, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m, - bool isConstraintPass=false) - { - computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt,scratch_r,scratch_v,scratch_m,isConstraintPass); - } - - // calcAccelerationDeltasMultiDof - // input: force vector (in same format as jacobian, i.e.: - // 3 torque values, 3 force values, num_links joint torque values) - // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values - // (existing contents of output array are replaced) - // calcAccelerationDeltasMultiDof must have been called first. - void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v) const; - - - void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier) - { - for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { - m_deltaV[dof] += delta_vee[dof] * multiplier; - } - } - void processDeltaVeeMultiDof2() - { - applyDeltaVeeMultiDof(&m_deltaV[0],1); - - for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { - m_deltaV[dof] = 0.f; - } - } - - void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) - { - //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - // printf("%.4f ", delta_vee[dof]*multiplier); - //printf("\n"); - - //btScalar sum = 0; - //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - //{ - // sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier; - //} - //btScalar l = btSqrt(sum); - - //if (l>m_maxAppliedImpulse) - //{ - // multiplier *= m_maxAppliedImpulse/l; - //} - - for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { - m_realBuf[dof] += delta_vee[dof] * multiplier; - btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity); - } - } - - - - // timestep the positions (given current velocities). - void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); - - - // - // contacts - // - - // This routine fills out a contact constraint jacobian for this body. - // the 'normal' supplied must be -n for body1 or +n for body2 of the contact. - // 'normal' & 'contact_point' are both given in world coordinates. - - void fillContactJacobianMultiDof(int link, - const btVector3 &contact_point, - const btVector3 &normal, - btScalar *jac, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); } - - //a more general version of fillContactJacobianMultiDof which does not assume.. - //.. that the constraint in question is contact or, to be more precise, constrains linear velocity only - void fillConstraintJacobianMultiDof(int link, - const btVector3 &contact_point, - const btVector3 &normal_ang, - const btVector3 &normal_lin, - btScalar *jac, - btAlignedObjectArray &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &scratch_m) const; - - - // - // sleeping - // - void setCanSleep(bool canSleep) - { - m_canSleep = canSleep; - } - - bool getCanSleep()const - { - return m_canSleep; - } - - bool isAwake() const { return m_awake; } - void wakeUp(); - void goToSleep(); - void checkMotionAndSleepIfRequired(btScalar timestep); - - bool hasFixedBase() const - { - return m_fixedBase; - } - - int getCompanionId() const - { - return m_companionId; - } - void setCompanionId(int id) - { - //printf("for %p setCompanionId(%d)\n",this, id); - m_companionId = id; - } - - void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links - { - m_links.resize(numLinks); - } - - btScalar getLinearDamping() const - { - return m_linearDamping; - } - void setLinearDamping( btScalar damp) - { - m_linearDamping = damp; - } - btScalar getAngularDamping() const - { - return m_angularDamping; - } - void setAngularDamping( btScalar damp) - { - m_angularDamping = damp; - } - - bool getUseGyroTerm() const - { - return m_useGyroTerm; - } - void setUseGyroTerm(bool useGyro) - { - m_useGyroTerm = useGyro; - } - btScalar getMaxCoordinateVelocity() const - { - return m_maxCoordinateVelocity ; - } - void setMaxCoordinateVelocity(btScalar maxVel) - { - m_maxCoordinateVelocity = maxVel; - } - - btScalar getMaxAppliedImpulse() const - { - return m_maxAppliedImpulse; - } - void setMaxAppliedImpulse(btScalar maxImp) - { - m_maxAppliedImpulse = maxImp; - } - void setHasSelfCollision(bool hasSelfCollision) - { - m_hasSelfCollision = hasSelfCollision; - } - bool hasSelfCollision() const - { - return m_hasSelfCollision; - } - - - void finalizeMultiDof(); - - void useRK4Integration(bool use) { m_useRK4 = use; } - bool isUsingRK4Integration() const { return m_useRK4; } - void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; } - bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; } - - bool isPosUpdated() const - { - return __posUpdated; - } - void setPosUpdated(bool updated) - { - __posUpdated = updated; - } - - //internalNeedsJointFeedback is for internal use only - bool internalNeedsJointFeedback() const - { - return m_internalNeedsJointFeedback; - } - void forwardKinematics(btAlignedObjectArray& scratch_q,btAlignedObjectArray& scratch_m); - - void updateCollisionObjectWorldTransforms(btAlignedObjectArray& scratch_q,btAlignedObjectArray& scratch_m); - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; - - const char* getBaseName() const - { - return m_baseName; - } - ///memory of setBaseName needs to be manager by user - void setBaseName(const char* name) - { - m_baseName = name; - } - -private: - btMultiBody(const btMultiBody &); // not implemented - void operator=(const btMultiBody &); // not implemented - - void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; - - void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const; - void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const; - - void updateLinksDofOffsets() - { - int dofOffset = 0, cfgOffset = 0; - for(int bidx = 0; bidx < m_links.size(); ++bidx) - { - m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset; - dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount; - } - } - - void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; - - -private: - - btMultiBodyLinkCollider* m_baseCollider;//can be NULL - const char* m_baseName;//memory needs to be manager by user! - - btVector3 m_basePos; // position of COM of base (world frame) - btQuaternion m_baseQuat; // rotates world points into base frame - - btScalar m_baseMass; // mass of the base - btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) - - btVector3 m_baseForce; // external force applied to base. World frame. - btVector3 m_baseTorque; // external torque applied to base. World frame. - - btVector3 m_baseConstraintForce; // external force applied to base. World frame. - btVector3 m_baseConstraintTorque; // external torque applied to base. World frame. - - btAlignedObjectArray m_links; // array of m_links, excluding the base. index from 0 to num_links-1. - btAlignedObjectArray m_colliders; - - - // - // realBuf: - // offset size array - // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized] - // 6+num_links num_links D - // - // vectorBuf: - // offset size array - // 0 num_links h_top - // num_links num_links h_bottom - // - // matrixBuf: - // offset size array - // 0 num_links+1 rot_from_parent - // - btAlignedObjectArray m_deltaV; - btAlignedObjectArray m_realBuf; - btAlignedObjectArray m_vectorBuf; - btAlignedObjectArray m_matrixBuf; - - - btMatrix3x3 m_cachedInertiaTopLeft; - btMatrix3x3 m_cachedInertiaTopRight; - btMatrix3x3 m_cachedInertiaLowerLeft; - btMatrix3x3 m_cachedInertiaLowerRight; - - bool m_fixedBase; - - // Sleep parameters. - bool m_awake; - bool m_canSleep; - btScalar m_sleepTimer; - - int m_companionId; - btScalar m_linearDamping; - btScalar m_angularDamping; - bool m_useGyroTerm; - btScalar m_maxAppliedImpulse; - btScalar m_maxCoordinateVelocity; - bool m_hasSelfCollision; - - bool __posUpdated; - int m_dofCount, m_posVarCnt; - bool m_useRK4, m_useGlobalVelocities; - - ///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only - bool m_internalNeedsJointFeedback; -}; - -struct btMultiBodyLinkDoubleData -{ - btQuaternionDoubleData m_zeroRotParentToThis; - btVector3DoubleData m_parentComToThisComOffset; - btVector3DoubleData m_thisPivotToThisComOffset; - btVector3DoubleData m_jointAxisTop[6]; - btVector3DoubleData m_jointAxisBottom[6]; - - - char *m_linkName; - char *m_jointName; - btCollisionObjectDoubleData *m_linkCollider; - - btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal) - double m_linkMass; - int m_parentIndex; - int m_jointType; - - - - - int m_dofCount; - int m_posVarCount; - double m_jointPos[7]; - double m_jointVel[6]; - double m_jointTorque[6]; - - - -}; - -struct btMultiBodyLinkFloatData -{ - btQuaternionFloatData m_zeroRotParentToThis; - btVector3FloatData m_parentComToThisComOffset; - btVector3FloatData m_thisPivotToThisComOffset; - btVector3FloatData m_jointAxisTop[6]; - btVector3FloatData m_jointAxisBottom[6]; - - - char *m_linkName; - char *m_jointName; - btCollisionObjectFloatData *m_linkCollider; - - btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal) - int m_dofCount; - float m_linkMass; - int m_parentIndex; - int m_jointType; - - - - float m_jointPos[7]; - float m_jointVel[6]; - float m_jointTorque[6]; - int m_posVarCount; - - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btMultiBodyDoubleData -{ - char *m_baseName; - btMultiBodyLinkDoubleData *m_links; - btCollisionObjectDoubleData *m_baseCollider; - - btTransformDoubleData m_baseWorldTransform; - btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal) - - int m_numLinks; - double m_baseMass; - - char m_padding[4]; - -}; - -///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btMultiBodyFloatData -{ - char *m_baseName; - btMultiBodyLinkFloatData *m_links; - btCollisionObjectFloatData *m_baseCollider; - btTransformFloatData m_baseWorldTransform; - btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal) - - float m_baseMass; - int m_numLinks; -}; - - - -#endif +/* + * PURPOSE: + * Class representing an articulated rigid body. Stores the body's + * current state, allows forces and torques to be set, handles + * timestepping and implements Featherstone's algorithm. + * + * COPYRIGHT: + * Copyright (C) Stephen Thompson, , 2011-2013 + * Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) + * Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements + + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + + */ + + +#ifndef BT_MULTIBODY_H +#define BT_MULTIBODY_H + +#include "LinearMath/btScalar.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btAlignedObjectArray.h" + + +///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms +#ifdef BT_USE_DOUBLE_PRECISION + #define btMultiBodyData btMultiBodyDoubleData + #define btMultiBodyDataName "btMultiBodyDoubleData" + #define btMultiBodyLinkData btMultiBodyLinkDoubleData + #define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData" +#else + #define btMultiBodyData btMultiBodyFloatData + #define btMultiBodyDataName "btMultiBodyFloatData" + #define btMultiBodyLinkData btMultiBodyLinkFloatData + #define btMultiBodyLinkDataName "btMultiBodyLinkFloatData" +#endif //BT_USE_DOUBLE_PRECISION + +#include "btMultiBodyLink.h" +class btMultiBodyLinkCollider; + +ATTRIBUTE_ALIGNED16(class) btMultiBody +{ +public: + + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + // + // initialization + // + + btMultiBody(int n_links, // NOT including the base + btScalar mass, // mass of base + const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal + bool fixedBase, // whether the base is fixed (true) or can move (false) + bool canSleep, bool deprecatedMultiDof=true); + + + virtual ~btMultiBody(); + + //note: fixed link collision with parent is always disabled + void setupFixed(int linkIndex, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true); + + + void setupPrismatic(int i, + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, + const btVector3 &jointAxis, + const btVector3 &parentComToThisPivotOffset, + const btVector3 &thisPivotToThisComOffset, + bool disableParentCollision); + + void setupRevolute(int linkIndex, // 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, + int parentIndex, + const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &jointAxis, // in my frame + const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame + const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame + bool disableParentCollision=false); + + void setupSpherical(int linkIndex, // 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame + const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame + bool disableParentCollision=false); + + void setupPlanar(int i, // 0 to num_links-1 + btScalar mass, + const btVector3 &inertia, + int parent, + const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 + const btVector3 &rotationAxis, + const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame + bool disableParentCollision=false); + + const btMultibodyLink& getLink(int index) const + { + return m_links[index]; + } + + btMultibodyLink& getLink(int index) + { + return m_links[index]; + } + + + void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base + { + m_baseCollider = collider; + } + const btMultiBodyLinkCollider* getBaseCollider() const + { + return m_baseCollider; + } + btMultiBodyLinkCollider* getBaseCollider() + { + return m_baseCollider; + } + + btMultiBodyLinkCollider* getLinkCollider(int index) + { + if (index >= 0 && index < getNumLinks()) + { + return getLink(index).m_collider; + } + return 0; + } + + // + // get parent + // input: link num from 0 to num_links-1 + // output: link num from 0 to num_links-1, OR -1 to mean the base. + // + int getParent(int link_num) const; + + + // + // get number of m_links, masses, moments of inertia + // + + int getNumLinks() const { return m_links.size(); } + int getNumDofs() const { return m_dofCount; } + int getNumPosVars() const { return m_posVarCnt; } + btScalar getBaseMass() const { return m_baseMass; } + const btVector3 & getBaseInertia() const { return m_baseInertia; } + btScalar getLinkMass(int i) const; + const btVector3 & getLinkInertia(int i) const; + + + + // + // change mass (incomplete: can only change base mass and inertia at present) + // + + void setBaseMass(btScalar mass) { m_baseMass = mass; } + void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; } + + + // + // get/set pos/vel/rot/omega for the base link + // + + const btVector3 & getBasePos() const { return m_basePos; } // in world frame + const btVector3 getBaseVel() const + { + return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]); + } // in world frame + const btQuaternion & getWorldToBaseRot() const + { + return m_baseQuat; + } // rotates world vectors into base frame + btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame + + void setBasePos(const btVector3 &pos) + { + m_basePos = pos; + } + + void setBaseWorldTransform(const btTransform& tr) + { + setBasePos(tr.getOrigin()); + setWorldToBaseRot(tr.getRotation().inverse()); + + } + + btTransform getBaseWorldTransform() const + { + btTransform tr; + tr.setOrigin(getBasePos()); + tr.setRotation(getWorldToBaseRot().inverse()); + return tr; + } + + void setBaseVel(const btVector3 &vel) + { + + m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; + } + void setWorldToBaseRot(const btQuaternion &rot) + { + m_baseQuat = rot; //m_baseQuat asumed to ba alias!? + } + void setBaseOmega(const btVector3 &omega) + { + m_realBuf[0]=omega[0]; + m_realBuf[1]=omega[1]; + m_realBuf[2]=omega[2]; + } + + + // + // get/set pos/vel for child m_links (i = 0 to num_links-1) + // + + btScalar getJointPos(int i) const; + btScalar getJointVel(int i) const; + + btScalar * getJointVelMultiDof(int i); + btScalar * getJointPosMultiDof(int i); + + const btScalar * getJointVelMultiDof(int i) const ; + const btScalar * getJointPosMultiDof(int i) const ; + + void setJointPos(int i, btScalar q); + void setJointVel(int i, btScalar qdot); + void setJointPosMultiDof(int i, btScalar *q); + void setJointVelMultiDof(int i, btScalar *qdot); + + + + // + // direct access to velocities as a vector of 6 + num_links elements. + // (omega first, then v, then joint velocities.) + // + const btScalar * getVelocityVector() const + { + return &m_realBuf[0]; + } +/* btScalar * getVelocityVector() + { + return &real_buf[0]; + } + */ + + // + // get the frames of reference (positions and orientations) of the child m_links + // (i = 0 to num_links-1) + // + + const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords + const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + + + // + // transform vectors in local frame of link i to world frame (or vice versa) + // + btVector3 localPosToWorld(int i, const btVector3 &vec) const; + btVector3 localDirToWorld(int i, const btVector3 &vec) const; + btVector3 worldPosToLocal(int i, const btVector3 &vec) const; + btVector3 worldDirToLocal(int i, const btVector3 &vec) const; + + // + // transform a frame in local coordinate to a frame in world coordinate + // + btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const; + + // + // calculate kinetic energy and angular momentum + // useful for debugging. + // + + btScalar getKineticEnergy() const; + btVector3 getAngularMomentum() const; + + + // + // set external forces and torques. Note all external forces/torques are given in the WORLD frame. + // + + void clearForcesAndTorques(); + void clearConstraintForces(); + + void clearVelocities(); + + void addBaseForce(const btVector3 &f) + { + m_baseForce += f; + } + void addBaseTorque(const btVector3 &t) { m_baseTorque += t; } + void addLinkForce(int i, const btVector3 &f); + void addLinkTorque(int i, const btVector3 &t); + + void addBaseConstraintForce(const btVector3 &f) + { + m_baseConstraintForce += f; + } + void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; } + void addLinkConstraintForce(int i, const btVector3 &f); + void addLinkConstraintTorque(int i, const btVector3 &t); + + +void addJointTorque(int i, btScalar Q); + void addJointTorqueMultiDof(int i, int dof, btScalar Q); + void addJointTorqueMultiDof(int i, const btScalar *Q); + + const btVector3 & getBaseForce() const { return m_baseForce; } + const btVector3 & getBaseTorque() const { return m_baseTorque; } + const btVector3 & getLinkForce(int i) const; + const btVector3 & getLinkTorque(int i) const; + btScalar getJointTorque(int i) const; + btScalar * getJointTorqueMultiDof(int i); + + + // + // dynamics routines. + // + + // timestep the velocities (given the external forces/torques set using addBaseForce etc). + // also sets up caches for calcAccelerationDeltas. + // + // Note: the caller must provide three vectors which are used as + // temporary scratch space. The idea here is to reduce dynamic + // memory allocation: the same scratch vectors can be re-used + // again and again for different Multibodies, instead of each + // btMultiBody allocating (and then deallocating) their own + // individual scratch buffers. This gives a considerable speed + // improvement, at least on Windows (where dynamic memory + // allocation appears to be fairly slow). + // + + + void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m, + bool isConstraintPass=false + ); + +///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead + void stepVelocitiesMultiDof(btScalar dt, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m, + bool isConstraintPass=false) + { + computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt,scratch_r,scratch_v,scratch_m,isConstraintPass); + } + + // calcAccelerationDeltasMultiDof + // input: force vector (in same format as jacobian, i.e.: + // 3 torque values, 3 force values, num_links joint torque values) + // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values + // (existing contents of output array are replaced) + // calcAccelerationDeltasMultiDof must have been called first. + void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v) const; + + + void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier) + { + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_deltaV[dof] += delta_vee[dof] * multiplier; + } + } + void processDeltaVeeMultiDof2() + { + applyDeltaVeeMultiDof(&m_deltaV[0],1); + + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_deltaV[dof] = 0.f; + } + } + + void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) + { + //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + // printf("%.4f ", delta_vee[dof]*multiplier); + //printf("\n"); + + //btScalar sum = 0; + //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + //{ + // sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier; + //} + //btScalar l = btSqrt(sum); + + //if (l>m_maxAppliedImpulse) + //{ + // multiplier *= m_maxAppliedImpulse/l; + //} + + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { + m_realBuf[dof] += delta_vee[dof] * multiplier; + btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity); + } + } + + + + // timestep the positions (given current velocities). + void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); + + + // + // contacts + // + + // This routine fills out a contact constraint jacobian for this body. + // the 'normal' supplied must be -n for body1 or +n for body2 of the contact. + // 'normal' & 'contact_point' are both given in world coordinates. + + void fillContactJacobianMultiDof(int link, + const btVector3 &contact_point, + const btVector3 &normal, + btScalar *jac, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); } + + //a more general version of fillContactJacobianMultiDof which does not assume.. + //.. that the constraint in question is contact or, to be more precise, constrains linear velocity only + void fillConstraintJacobianMultiDof(int link, + const btVector3 &contact_point, + const btVector3 &normal_ang, + const btVector3 &normal_lin, + btScalar *jac, + btAlignedObjectArray &scratch_r, + btAlignedObjectArray &scratch_v, + btAlignedObjectArray &scratch_m) const; + + + // + // sleeping + // + void setCanSleep(bool canSleep) + { + m_canSleep = canSleep; + } + + bool getCanSleep()const + { + return m_canSleep; + } + + bool isAwake() const { return m_awake; } + void wakeUp(); + void goToSleep(); + void checkMotionAndSleepIfRequired(btScalar timestep); + + bool hasFixedBase() const + { + return m_fixedBase; + } + + int getCompanionId() const + { + return m_companionId; + } + void setCompanionId(int id) + { + //printf("for %p setCompanionId(%d)\n",this, id); + m_companionId = id; + } + + void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links + { + m_links.resize(numLinks); + } + + btScalar getLinearDamping() const + { + return m_linearDamping; + } + void setLinearDamping( btScalar damp) + { + m_linearDamping = damp; + } + btScalar getAngularDamping() const + { + return m_angularDamping; + } + void setAngularDamping( btScalar damp) + { + m_angularDamping = damp; + } + + bool getUseGyroTerm() const + { + return m_useGyroTerm; + } + void setUseGyroTerm(bool useGyro) + { + m_useGyroTerm = useGyro; + } + btScalar getMaxCoordinateVelocity() const + { + return m_maxCoordinateVelocity ; + } + void setMaxCoordinateVelocity(btScalar maxVel) + { + m_maxCoordinateVelocity = maxVel; + } + + btScalar getMaxAppliedImpulse() const + { + return m_maxAppliedImpulse; + } + void setMaxAppliedImpulse(btScalar maxImp) + { + m_maxAppliedImpulse = maxImp; + } + void setHasSelfCollision(bool hasSelfCollision) + { + m_hasSelfCollision = hasSelfCollision; + } + bool hasSelfCollision() const + { + return m_hasSelfCollision; + } + + + void finalizeMultiDof(); + + void useRK4Integration(bool use) { m_useRK4 = use; } + bool isUsingRK4Integration() const { return m_useRK4; } + void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; } + bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; } + + bool isPosUpdated() const + { + return __posUpdated; + } + void setPosUpdated(bool updated) + { + __posUpdated = updated; + } + + //internalNeedsJointFeedback is for internal use only + bool internalNeedsJointFeedback() const + { + return m_internalNeedsJointFeedback; + } + void forwardKinematics(btAlignedObjectArray& scratch_q,btAlignedObjectArray& scratch_m); + + void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; + + void updateCollisionObjectWorldTransforms(btAlignedObjectArray& scratch_q,btAlignedObjectArray& scratch_m); + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; + + const char* getBaseName() const + { + return m_baseName; + } + ///memory of setBaseName needs to be manager by user + void setBaseName(const char* name) + { + m_baseName = name; + } + + ///users can point to their objects, userPointer is not used by Bullet + void* getUserPointer() const + { + return m_userObjectPointer; + } + + int getUserIndex() const + { + return m_userIndex; + } + + int getUserIndex2() const + { + return m_userIndex2; + } + ///users can point to their objects, userPointer is not used by Bullet + void setUserPointer(void* userPointer) + { + m_userObjectPointer = userPointer; + } + + ///users can point to their objects, userPointer is not used by Bullet + void setUserIndex(int index) + { + m_userIndex = index; + } + + void setUserIndex2(int index) + { + m_userIndex2 = index; + } + +private: + btMultiBody(const btMultiBody &); // not implemented + void operator=(const btMultiBody &); // not implemented + + + void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, btScalar result[6]) const; + void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const; + + void updateLinksDofOffsets() + { + int dofOffset = 0, cfgOffset = 0; + for(int bidx = 0; bidx < m_links.size(); ++bidx) + { + m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset; + dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount; + } + } + + void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; + + +private: + + btMultiBodyLinkCollider* m_baseCollider;//can be NULL + const char* m_baseName;//memory needs to be manager by user! + + btVector3 m_basePos; // position of COM of base (world frame) + btQuaternion m_baseQuat; // rotates world points into base frame + + btScalar m_baseMass; // mass of the base + btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) + + btVector3 m_baseForce; // external force applied to base. World frame. + btVector3 m_baseTorque; // external torque applied to base. World frame. + + btVector3 m_baseConstraintForce; // external force applied to base. World frame. + btVector3 m_baseConstraintTorque; // external torque applied to base. World frame. + + btAlignedObjectArray m_links; // array of m_links, excluding the base. index from 0 to num_links-1. + + + // + // realBuf: + // offset size array + // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized] + // 6+num_links num_links D + // + // vectorBuf: + // offset size array + // 0 num_links h_top + // num_links num_links h_bottom + // + // matrixBuf: + // offset size array + // 0 num_links+1 rot_from_parent + // + btAlignedObjectArray m_deltaV; + btAlignedObjectArray m_realBuf; + btAlignedObjectArray m_vectorBuf; + btAlignedObjectArray m_matrixBuf; + + + btMatrix3x3 m_cachedInertiaTopLeft; + btMatrix3x3 m_cachedInertiaTopRight; + btMatrix3x3 m_cachedInertiaLowerLeft; + btMatrix3x3 m_cachedInertiaLowerRight; + bool m_cachedInertiaValid; + + bool m_fixedBase; + + // Sleep parameters. + bool m_awake; + bool m_canSleep; + btScalar m_sleepTimer; + + void* m_userObjectPointer; + int m_userIndex2; + int m_userIndex; + + int m_companionId; + btScalar m_linearDamping; + btScalar m_angularDamping; + bool m_useGyroTerm; + btScalar m_maxAppliedImpulse; + btScalar m_maxCoordinateVelocity; + bool m_hasSelfCollision; + + bool __posUpdated; + int m_dofCount, m_posVarCnt; + + bool m_useRK4, m_useGlobalVelocities; + //for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis + //https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing + + ///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only + bool m_internalNeedsJointFeedback; +}; + +struct btMultiBodyLinkDoubleData +{ + btQuaternionDoubleData m_zeroRotParentToThis; + btVector3DoubleData m_parentComToThisPivotOffset; + btVector3DoubleData m_thisPivotToThisComOffset; + btVector3DoubleData m_jointAxisTop[6]; + btVector3DoubleData m_jointAxisBottom[6]; + + btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal) + btVector3DoubleData m_absFrameTotVelocityTop; + btVector3DoubleData m_absFrameTotVelocityBottom; + btVector3DoubleData m_absFrameLocVelocityTop; + btVector3DoubleData m_absFrameLocVelocityBottom; + + double m_linkMass; + int m_parentIndex; + int m_jointType; + + int m_dofCount; + int m_posVarCount; + double m_jointPos[7]; + double m_jointVel[6]; + double m_jointTorque[6]; + + double m_jointDamping; + double m_jointFriction; + double m_jointLowerLimit; + double m_jointUpperLimit; + double m_jointMaxForce; + double m_jointMaxVelocity; + + char *m_linkName; + char *m_jointName; + btCollisionObjectDoubleData *m_linkCollider; + char *m_paddingPtr; + +}; + +struct btMultiBodyLinkFloatData +{ + btQuaternionFloatData m_zeroRotParentToThis; + btVector3FloatData m_parentComToThisPivotOffset; + btVector3FloatData m_thisPivotToThisComOffset; + btVector3FloatData m_jointAxisTop[6]; + btVector3FloatData m_jointAxisBottom[6]; + btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal) + btVector3FloatData m_absFrameTotVelocityTop; + btVector3FloatData m_absFrameTotVelocityBottom; + btVector3FloatData m_absFrameLocVelocityTop; + btVector3FloatData m_absFrameLocVelocityBottom; + + int m_dofCount; + float m_linkMass; + int m_parentIndex; + int m_jointType; + + + + float m_jointPos[7]; + float m_jointVel[6]; + float m_jointTorque[6]; + int m_posVarCount; + float m_jointDamping; + float m_jointFriction; + float m_jointLowerLimit; + float m_jointUpperLimit; + float m_jointMaxForce; + float m_jointMaxVelocity; + + char *m_linkName; + char *m_jointName; + btCollisionObjectFloatData *m_linkCollider; + char *m_paddingPtr; + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btMultiBodyDoubleData +{ + btVector3DoubleData m_baseWorldPosition; + btQuaternionDoubleData m_baseWorldOrientation; + btVector3DoubleData m_baseLinearVelocity; + btVector3DoubleData m_baseAngularVelocity; + btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal) + double m_baseMass; + int m_numLinks; + char m_padding[4]; + + char *m_baseName; + btMultiBodyLinkDoubleData *m_links; + btCollisionObjectDoubleData *m_baseCollider; + + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btMultiBodyFloatData +{ + btVector3FloatData m_baseWorldPosition; + btQuaternionFloatData m_baseWorldOrientation; + btVector3FloatData m_baseLinearVelocity; + btVector3FloatData m_baseAngularVelocity; + + btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal) + float m_baseMass; + int m_numLinks; + + char *m_baseName; + btMultiBodyLinkFloatData *m_links; + btCollisionObjectFloatData *m_baseCollider; + +}; + + + +#endif diff --git a/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp new file mode 100644 index 000000000000..9f61874b8372 --- /dev/null +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -0,0 +1,419 @@ +#include "btMultiBodyConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) + + + +btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral) + :m_bodyA(bodyA), + m_bodyB(bodyB), + m_linkA(linkA), + m_linkB(linkB), + m_numRows(numRows), + m_jacSizeA(0), + m_jacSizeBoth(0), + m_isUnilateral(isUnilateral), + m_numDofsFinalized(-1), + m_maxAppliedImpulse(100) +{ + +} + +void btMultiBodyConstraint::updateJacobianSizes() +{ + if(m_bodyA) + { + m_jacSizeA = (6 + m_bodyA->getNumDofs()); + } + + if(m_bodyB) + { + m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs(); + } + else + m_jacSizeBoth = m_jacSizeA; +} + +void btMultiBodyConstraint::allocateJacobiansMultiDof() +{ + updateJacobianSizes(); + + m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); + m_data.resize((2 + m_jacSizeBoth) * m_numRows); +} + +btMultiBodyConstraint::~btMultiBodyConstraint() +{ +} + +void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +{ + for (int i = 0; i < ndof; ++i) + data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; +} + +btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + btScalar* jacOrgA, btScalar* jacOrgB, + const btVector3& constraintNormalAng, + const btVector3& constraintNormalLin, + const btVector3& posAworld, const btVector3& posBworld, + btScalar posError, + const btContactSolverInfo& infoGlobal, + btScalar lowerLimit, btScalar upperLimit, + bool angConstraint, + btScalar relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + solverConstraint.m_multiBodyA = m_bodyA; + solverConstraint.m_multiBodyB = m_bodyB; + solverConstraint.m_linkA = m_linkA; + solverConstraint.m_linkB = m_linkB; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); + btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary + if (bodyA) + rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin(); + + if (multiBodyA) + { + if (solverConstraint.m_linkA<0) + { + rel_pos1 = posAworld - multiBodyA->getBasePos(); + } else + { + rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); + } + + const int ndofA = multiBodyA->getNumDofs() + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom + //resize.. + solverConstraint.m_jacAindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofA); + //copy/determine + if(jacOrgA) + { + for (int i=0;ifillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + } + + //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) + //resize.. + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + //determine.. + multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + + btVector3 torqueAxis0; + if (angConstraint) { + torqueAxis0 = constraintNormalAng; + } + else { + torqueAxis0 = rel_pos1.cross(constraintNormalLin); + + } + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = constraintNormalLin; + } + else //if(rb0) + { + btVector3 torqueAxis0; + if (angConstraint) { + torqueAxis0 = constraintNormalAng; + } + else { + torqueAxis0 = rel_pos1.cross(constraintNormalLin); + } + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = constraintNormalLin; + } + + if (multiBodyB) + { + if (solverConstraint.m_linkB<0) + { + rel_pos2 = posBworld - multiBodyB->getBasePos(); + } else + { + rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); + } + + const int ndofB = multiBodyB->getNumDofs() + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); + } + + //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom + //resize.. + solverConstraint.m_jacBindex = data.m_jacobians.size(); + data.m_jacobians.resize(data.m_jacobians.size()+ndofB); + //copy/determine.. + if(jacOrgB) + { + for (int i=0;ifillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + } + + //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) + //resize.. + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + //determine.. + multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); + + btVector3 torqueAxis1; + if (angConstraint) { + torqueAxis1 = constraintNormalAng; + } + else { + torqueAxis1 = rel_pos2.cross(constraintNormalLin); + } + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -constraintNormalLin; + } + else //if(rb1) + { + btVector3 torqueAxis1; + if (angConstraint) { + torqueAxis1 = constraintNormalAng; + } + else { + torqueAxis1 = rel_pos2.cross(constraintNormalLin); + } + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -constraintNormalLin; + } + { + + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* deltaVelA = 0; + btScalar* deltaVelB = 0; + int ndofA = 0; + //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i]) + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l = deltaVelA[i]; + denom0 += j*l; + } + } + else if(rb0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + if (angConstraint) { + denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA); + } + else { + denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec); + } + } + // + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l = deltaVelB[i]; + denom1 += j*l; + } + + } + else if(rb1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + if (angConstraint) { + denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB); + } + else { + denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec); + } + } + + // + btScalar d = denom0+denom1; + if (d>SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation/(d); + } + else + { + //disable the constraint row to handle singularity/redundant constraint + solverConstraint.m_jacDiagABInv = 0.f; + } + } + + + //compute rhs and remaining solverConstraint fields + btScalar penetration = isFriction? 0 : posError; + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } + else if(rb0) + { + rel_vel += rb0->getLinearVelocity().dot(solverConstraint.m_contactNormal1); + rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal); + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumDofs() + 6; + btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } + else if(rb1) + { + rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2); + rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal); + } + + solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; + } + + + ///warm starting (or zero if disabled) + /* + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + + if (solverConstraint.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); + } else + { + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + } + if (multiBodyB) + { + btScalar impulse = solverConstraint.m_appliedImpulse; + btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); + } else + { + if (rb1) + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); + } + } + } else + */ + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + + btScalar positionalError = 0.f; + btScalar velocityError = desiredVelocity - rel_vel;// * damping; + + + btScalar erp = infoGlobal.m_erp2; + + //split impulse is not implemented yet for btMultiBody* + //if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + + positionalError = -penetration * erp/infoGlobal.m_timeStep; + + btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + + //split impulse is not implemented yet for btMultiBody* + + // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + + } + /*else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + */ + + solverConstraint.m_cfm = 0.f; + solverConstraint.m_lowerLimit = lowerLimit; + solverConstraint.m_upperLimit = upperLimit; + } + + return rel_vel; + +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h similarity index 83% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 3b32b46e911f..a2ae57127332 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -39,7 +39,7 @@ struct btMultiBodyJacobianData }; -class btMultiBodyConstraint +ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraint { protected: @@ -66,26 +66,36 @@ class btMultiBodyConstraint btAlignedObjectArray m_data; void applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof); - + btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, btMultiBodyJacobianData& data, - btScalar* jacOrgA, btScalar* jacOrgB, - const btVector3& contactNormalOnB, + btScalar* jacOrgA, btScalar* jacOrgB, + const btVector3& constraintNormalAng, + + const btVector3& constraintNormalLin, const btVector3& posAworld, const btVector3& posBworld, btScalar posError, const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, + btScalar lowerLimit, btScalar upperLimit, + bool angConstraint = false, + btScalar relaxation = 1.f, bool isFriction = false, btScalar desiredVelocity=0, btScalar cfmSlip=0); public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral); virtual ~btMultiBodyConstraint(); void updateJacobianSizes(); void allocateJacobiansMultiDof(); + //many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later. + virtual void setFrameInB(const btMatrix3x3& frameInB) {} + virtual void setPivotInB(const btVector3& pivotInB){} + virtual void finalizeMultiDof()=0; virtual int getIslandIdA() const =0; @@ -109,11 +119,20 @@ class btMultiBodyConstraint return m_bodyB; } + int getLinkA() const + { + return m_linkA; + } + int getLinkB() const + { + return m_linkB; + } void internalSetAppliedImpulse(int dof, btScalar appliedImpulse) { btAssert(dof>=0); btAssert(dof < getNumRows()); m_data[dof] = appliedImpulse; + } btScalar getAppliedImpulse(int dof) @@ -172,6 +191,12 @@ class btMultiBodyConstraint virtual void debugDraw(class btIDebugDraw* drawer)=0; + virtual void setGearRatio(btScalar ratio) {} + virtual void setGearAuxLink(int gearAuxLink) {} + virtual void setRelativePositionTarget(btScalar relPosTarget){} + virtual void setErp(btScalar erp){} + + }; #endif //BT_MULTIBODY_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp similarity index 52% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 4f66b20b2c1b..8c6cb6aa8aec 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -26,16 +26,21 @@ subject to the following restrictions: btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { - btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); //solve featherstone non-contact constraints //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); + for (int j=0;jsetPosUpdated(false); if(constraint.m_multiBodyB) @@ -43,41 +48,117 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl } //solve featherstone normal contact - for (int j=0;jsetPosUpdated(false); if(constraint.m_multiBodyB) constraint.m_multiBodyB->setPosUpdated(false); } - + //solve featherstone frictional contact + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)) + { + for (int j1 = 0; j1m_multiBodyTorsionalFrictionContactConstraints.size(); j1++) + { + if (iteration < infoGlobal.m_numIterations) + { + int index = j1;//iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1; - for (int j=0;jm_multiBodyFrictionContactConstraints.size();j++) + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + //adjust friction limits here + if (totalImpulse>btScalar(0)) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); + leastSquaredResidual += residual*residual; + + if (frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if (frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + } + } + } + + for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++) + { + if (iteration < infoGlobal.m_numIterations) + { + int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; + + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + j1++; + int index2 = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyFrictionContactConstraints[index2]; + btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex); + + if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction*totalImpulse); + frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction*totalImpulse; + btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB); + leastSquaredResidual += residual*residual; + + if (frictionConstraintB.m_multiBodyA) + frictionConstraintB.m_multiBodyA->setPosUpdated(false); + if (frictionConstraintB.m_multiBodyB) + frictionConstraintB.m_multiBodyB->setPosUpdated(false); + + if (frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if (frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + } + } + } + + + } + else { - if (iteration < infoGlobal.m_numIterations) + for (int j1 = 0; j1m_multiBodyFrictionContactConstraints.size(); j1++) { - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; - btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; - //adjust friction limits here - if (totalImpulse>btScalar(0)) + if (iteration < infoGlobal.m_numIterations) { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; - resolveSingleConstraintRowGeneric(frictionConstraint); - - if(frictionConstraint.m_multiBodyA) - frictionConstraint.m_multiBodyA->setPosUpdated(false); - if(frictionConstraint.m_multiBodyB) - frictionConstraint.m_multiBodyB->setPosUpdated(false); + int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + //adjust friction limits here + if (totalImpulse>btScalar(0)) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); + leastSquaredResidual += residual*residual; + + if (frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if (frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + } } } } - return val; + return leastSquaredResidual; } btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) @@ -85,6 +166,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb m_multiBodyNonContactConstraints.resize(0); m_multiBodyNormalContactConstraints.resize(0); m_multiBodyFrictionContactConstraints.resize(0); + m_multiBodyTorsionalFrictionContactConstraints.resize(0); + m_data.m_jacobians.resize(0); m_data.m_deltaVelocitiesUnitImpulse.resize(0); m_data.m_deltaVelocities.resize(0); @@ -109,85 +192,268 @@ void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar im m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; } -void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) +btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c) { - btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - btScalar deltaVelADotn=0; - btScalar deltaVelBDotn=0; + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; + btScalar deltaVelADotn = 0; + btScalar deltaVelBDotn = 0; btSolverBody* bodyA = 0; btSolverBody* bodyB = 0; - int ndofA=0; - int ndofB=0; + int ndofA = 0; + int ndofB = 0; if (c.m_multiBodyA) { - ndofA = c.m_multiBodyA->getNumDofs() + 6; - for (int i = 0; i < ndofA; ++i) - deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i]; - } else if(c.m_solverBodyIdA >= 0) + ndofA = c.m_multiBodyA->getNumDofs() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i]; + } + else if (c.m_solverBodyIdA >= 0) { bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA]; - deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); } if (c.m_multiBodyB) { - ndofB = c.m_multiBodyB->getNumDofs() + 6; - for (int i = 0; i < ndofB; ++i) - deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i]; - } else if(c.m_solverBodyIdB >= 0) + ndofB = c.m_multiBodyB->getNumDofs() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i]; + } + else if (c.m_solverBodyIdB >= 0) { bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB]; - deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); } - - deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom - deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; + + deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; - + if (sum < c.m_lowerLimit) { - deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse; c.m_appliedImpulse = c.m_lowerLimit; } - else if (sum > c.m_upperLimit) + else if (sum > c.m_upperLimit) { - deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + deltaImpulse = c.m_upperLimit - c.m_appliedImpulse; c.m_appliedImpulse = c.m_upperLimit; } else { c.m_appliedImpulse = sum; } - + if (c.m_multiBodyA) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse); #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } else if(c.m_solverBodyIdA >= 0) + } + else if (c.m_solverBodyIdA >= 0) { - bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse); } if (c.m_multiBodyB) { - applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } + else if (c.m_solverBodyIdB >= 0) + { + bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + } + return deltaImpulse; +} + + +btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1,const btMultiBodySolverConstraint& cB) +{ + int ndofA=0; + int ndofB=0; + btSolverBody* bodyA = 0; + btSolverBody* bodyB = 0; + btScalar deltaImpulseB = 0.f; + btScalar sumB = 0.f; + { + deltaImpulseB = cB.m_rhs-btScalar(cB.m_appliedImpulse)*cB.m_cfm; + btScalar deltaVelADotn=0; + btScalar deltaVelBDotn=0; + if (cB.m_multiBodyA) + { + ndofA = cB.m_multiBodyA->getNumDofs() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex+i]; + } else if(cB.m_solverBodyIdA >= 0) + { + bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA]; + deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + } + + if (cB.m_multiBodyB) + { + ndofB = cB.m_multiBodyB->getNumDofs() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex+i]; + } else if(cB.m_solverBodyIdB >= 0) + { + bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB]; + deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + } + + + deltaImpulseB -= deltaVelADotn*cB.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulseB -= deltaVelBDotn*cB.m_jacDiagABInv; + sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB; + } + + btScalar deltaImpulseA = 0.f; + btScalar sumA = 0.f; + const btMultiBodySolverConstraint& cA = cA1; + { + { + deltaImpulseA = cA.m_rhs-btScalar(cA.m_appliedImpulse)*cA.m_cfm; + btScalar deltaVelADotn=0; + btScalar deltaVelBDotn=0; + if (cA.m_multiBodyA) + { + ndofA = cA.m_multiBodyA->getNumDofs() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex+i]; + } else if(cA.m_solverBodyIdA >= 0) + { + bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA]; + deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity()); + } + + if (cA.m_multiBodyB) + { + ndofB = cA.m_multiBodyB->getNumDofs() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex+i]; + } else if(cA.m_solverBodyIdB >= 0) + { + bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB]; + deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity()); + } + + + deltaImpulseA -= deltaVelADotn*cA.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulseA -= deltaVelBDotn*cA.m_jacDiagABInv; + sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA; + } + } + + if (sumA*sumA+sumB*sumB>=cA.m_lowerLimit*cB.m_lowerLimit) + { + btScalar angle = btAtan2(sumA,sumB); + btScalar sumAclipped = btFabs(cA.m_lowerLimit*btSin(angle)); + btScalar sumBclipped = btFabs(cB.m_lowerLimit*btCos(angle)); + + + if (sumA < -sumAclipped) + { + deltaImpulseA = -sumAclipped - cA.m_appliedImpulse; + cA.m_appliedImpulse = -sumAclipped; + } + else if (sumA > sumAclipped) + { + deltaImpulseA = sumAclipped - cA.m_appliedImpulse; + cA.m_appliedImpulse = sumAclipped; + } + else + { + cA.m_appliedImpulse = sumA; + } + + if (sumB < -sumBclipped) + { + deltaImpulseB = -sumBclipped - cB.m_appliedImpulse; + cB.m_appliedImpulse = -sumBclipped; + } + else if (sumB > sumBclipped) + { + deltaImpulseB = sumBclipped - cB.m_appliedImpulse; + cB.m_appliedImpulse = sumBclipped; + } + else + { + cB.m_appliedImpulse = sumB; + } + //deltaImpulseA = sumAclipped-cA.m_appliedImpulse; + //cA.m_appliedImpulse = sumAclipped; + //deltaImpulseB = sumBclipped-cB.m_appliedImpulse; + //cB.m_appliedImpulse = sumBclipped; + } + else + { + cA.m_appliedImpulse = sumA; + cB.m_appliedImpulse = sumB; + } + + if (cA.m_multiBodyA) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA,cA.m_deltaVelAindex,ndofA); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } else if(cA.m_solverBodyIdA >= 0) + { + bodyA->internalApplyImpulse(cA.m_contactNormal1*bodyA->internalGetInvMass(),cA.m_angularComponentA,deltaImpulseA); + + } + if (cA.m_multiBodyB) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA,cA.m_deltaVelBindex,ndofB); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } else if(cA.m_solverBodyIdB >= 0) + { + bodyB->internalApplyImpulse(cA.m_contactNormal2*bodyB->internalGetInvMass(),cA.m_angularComponentB,deltaImpulseA); + } + + if (cB.m_multiBodyA) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB,cB.m_deltaVelAindex,ndofA); +#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations + //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity + cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB); +#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS + } else if(cB.m_solverBodyIdA >= 0) + { + bodyA->internalApplyImpulse(cB.m_contactNormal1*bodyA->internalGetInvMass(),cB.m_angularComponentA,deltaImpulseB); + } + if (cB.m_multiBodyB) + { + applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB,cB.m_deltaVelBindex,ndofB); #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity - c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB); #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS - } else if(c.m_solverBodyIdB >= 0) + } else if(cB.m_solverBodyIdB >= 0) { - bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + bodyB->internalApplyImpulse(cB.m_contactNormal2*bodyB->internalGetInvMass(),cB.m_angularComponentB,deltaImpulseB); } + return deltaImpulseA+deltaImpulseB; } @@ -221,10 +487,46 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (bodyB) rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - relaxation = 1.f; - + relaxation = infoGlobal.m_sor; + + btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; + //cfm = 1 / ( dt * kp + kd ) + //erp = dt * kp / ( dt * kp + kd ) + + btScalar cfm; + btScalar erp; + if (isFriction) + { + cfm = infoGlobal.m_frictionCFM; + erp = infoGlobal.m_frictionERP; + } else + { + cfm = infoGlobal.m_globalCfm; + erp = infoGlobal.m_erp2; + if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)) + { + if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) + cfm = cp.m_contactCFM; + if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP) + erp = cp.m_contactERP; + } else + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING) + { + btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 ); + if (denom < SIMD_EPSILON) + { + denom = SIMD_EPSILON; + } + cfm = btScalar(1) / denom; + erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; + } + } + } + + cfm *= invTimeStep; if (multiBodyA) { @@ -366,7 +668,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol - btScalar d = denom0+denom1; + btScalar d = denom0+denom1+cfm; if (d>SIMD_EPSILON) { solverConstraint.m_jacDiagABInv = relaxation/(d); @@ -384,8 +686,19 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop; - + btScalar distance = 0; + if (!isFriction) + { + distance = cp.getDistance()+infoGlobal.m_linearSlop; + } else + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) + { + distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal); + } + } + + btScalar rel_vel = 0.f; int ndofA = 0; int ndofB = 0; @@ -402,7 +715,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { if (rb0) { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + rel_vel += (rb0->getVelocityInLocalPoint(rel_pos1) + + (rb0->getTotalTorque()*rb0->getInvInertiaTensorWorld()*infoGlobal.m_timeStep).cross(rel_pos1)+ + rb0->getTotalForce()*rb0->getInvMass()*infoGlobal.m_timeStep).dot(solverConstraint.m_contactNormal1); } } if (multiBodyB) @@ -416,7 +731,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { if (rb1) { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + rel_vel += (rb1->getVelocityInLocalPoint(rel_pos2)+ + (rb1->getTotalTorque()*rb1->getInvInertiaTensorWorld()*infoGlobal.m_timeStep).cross(rel_pos2) + + rb1->getTotalForce()*rb1->getInvMass()*infoGlobal.m_timeStep).dot(solverConstraint.m_contactNormal2); } } @@ -424,7 +741,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if(!isFriction) { - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); if (restitution <= btScalar(0.)) { restitution = 0.f; @@ -476,21 +793,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btScalar positionalError = 0.f; btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction - - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - - if (penetration>0) + if (isFriction) { - positionalError = 0; - velocityError -= penetration / infoGlobal.m_timeStep; - + positionalError = -distance * erp/infoGlobal.m_timeStep; } else { - positionalError = -penetration * erp/infoGlobal.m_timeStep; + if (distance>0) + { + positionalError = 0; + velocityError -= distance / infoGlobal.m_timeStep; + + } else + { + positionalError = -distance * erp/infoGlobal.m_timeStep; + } } btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; @@ -498,37 +814,309 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if(!isFriction) { - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; - } else + } + /*else { //split position and velocity into rhs and m_rhsPenetration solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhsPenetration = penetrationImpulse; } - + */ solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; } else { - solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; } - solverConstraint.m_cfm = 0.f; //why not use cfmSlip? - } + solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv; + -} + } +} +void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& constraintNormal, + btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +{ + + BT_PROFILE("setupMultiBodyRollingFrictionConstraint"); + btVector3 rel_pos1; + btVector3 rel_pos2; + + btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; + btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; + btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; + + btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; + btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; + + if (bodyA) + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + if (bodyB) + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + + relaxation = infoGlobal.m_sor; + + // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; + + + if (multiBodyA) + { + if (solverConstraint.m_linkA<0) + { + rel_pos1 = pos1 - multiBodyA->getBasePos(); + } else + { + rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); + } + const int ndofA = multiBodyA->getNumDofs() + 6; + + solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (solverConstraint.m_deltaVelAindex <0) + { + solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size(); + multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA); + } else + { + btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + } + + solverConstraint.m_jacAindex = m_data.m_jacobians.size(); + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex]; + multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0,0,0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); + + btVector3 torqueAxis0 = -constraintNormal; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = btVector3(0,0,0); + } else + { + btVector3 torqueAxis0 = -constraintNormal; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = btVector3(0,0,0); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + } + + + + if (multiBodyB) + { + if (solverConstraint.m_linkB<0) + { + rel_pos2 = pos2 - multiBodyB->getBasePos(); + } else + { + rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); + } + + const int ndofB = multiBodyB->getNumDofs() + 6; + + solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (solverConstraint.m_deltaVelBindex <0) + { + solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size(); + multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); + m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB); + } + + solverConstraint.m_jacBindex = m_data.m_jacobians.size(); + + m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB); + m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size()); + + multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0,0,0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m); + multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); + + btVector3 torqueAxis1 = constraintNormal; + solverConstraint.m_relpos2CrossNormal = torqueAxis1; + solverConstraint.m_contactNormal2 = -btVector3(0,0,0); + + } else + { + btVector3 torqueAxis1 = constraintNormal; + solverConstraint.m_relpos2CrossNormal = torqueAxis1; + solverConstraint.m_contactNormal2 = -btVector3(0,0,0); + + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + } + + { + + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + btScalar* jacB = 0; + btScalar* jacA = 0; + btScalar* lambdaA =0; + btScalar* lambdaB =0; + int ndofA = 0; + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + btScalar j = jacA[i] ; + btScalar l =lambdaA[i]; + denom0 += j*l; + } + } else + { + if (rb0) + { + btVector3 iMJaA = rb0?rb0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0); + denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumDofs() + 6; + jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + btScalar j = jacB[i] ; + btScalar l =lambdaB[i]; + denom1 += j*l; + } + + } else + { + if (rb1) + { + btVector3 iMJaB = rb1?rb1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); + denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal); + } + } + + + + btScalar d = denom0+denom1+infoGlobal.m_globalCfm; + if (d>SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation/(d); + } else + { + //disable the constraint row to handle singularity/redundant constraint + solverConstraint.m_jacDiagABInv = 0.f; + } + + } + + + //compute rhs and remaining solverConstraint fields + + + + btScalar restitution = 0.f; + btScalar penetration = isFriction? 0 : cp.getDistance(); + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumDofs() + 6; + btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex]; + for (int i = 0; i < ndofA ; ++i) + rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; + } else + { + if (rb0) + { + btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; + rel_vel += solverConstraint.m_contactNormal1.dot(rb0?solverBodyA->m_linearVelocity+solverBodyA->m_externalForceImpulse:btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(rb0?solverBodyA->m_angularVelocity:btVector3(0,0,0)); + + } + } + if (multiBodyB) + { + ndofB = multiBodyB->getNumDofs() + 6; + btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } else + { + if (rb1) + { + btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; + rel_vel += solverConstraint.m_contactNormal2.dot(rb1?solverBodyB->m_linearVelocity+solverBodyB->m_externalForceImpulse:btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(rb1?solverBodyB->m_angularVelocity:btVector3(0,0,0)); + + } + } + + solverConstraint.m_friction =combinedTorsionalFriction; + + if(!isFriction) + { + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + } + } + } + + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + + btScalar velocityError = 0 - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction + + + + btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv; + + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = 0.f; + solverConstraint.m_lowerLimit = -solverConstraint.m_friction; + solverConstraint.m_upperLimit = solverConstraint.m_friction; + + solverConstraint.m_cfm = infoGlobal.m_globalCfm*solverConstraint.m_jacDiagABInv; + + + + } + +} btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { @@ -565,6 +1153,46 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo return solverConstraint; } +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +{ + BT_PROFILE("addMultiBodyRollingFrictionConstraint"); + + bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)); + + btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing(); + solverConstraint.m_orgConstraint = 0; + solverConstraint.m_orgDofIndex = -1; + + solverConstraint.m_frictionIndex = frictionIndex; + bool isFriction = true; + + const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); + const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); + + btMultiBody* mbA = fcA? fcA->m_multiBody : 0; + btMultiBody* mbB = fcB? fcB->m_multiBody : 0; + + int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); + int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_multiBodyA = mbA; + if (mbA) + solverConstraint.m_linkA = fcA->m_link; + + solverConstraint.m_multiBodyB = mbB; + if (mbB) + solverConstraint.m_linkB = fcB->m_link; + + solverConstraint.m_originalContactPoint = &cp; + + setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction,infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); + return solverConstraint; +} + void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) { const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); @@ -589,8 +1217,9 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero()))) // return; - - + //only a single rollingFriction per manifold + int rollingFriction=1; + for (int j=0;jgetNumContacts();j++) { @@ -631,49 +1260,11 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* #define ENABLE_FRICTION #ifdef ENABLE_FRICTION solverConstraint.m_frictionIndex = frictionIndex; -#if ROLLING_FRICTION - int rollingFriction=1; - btVector3 angVelA(0,0,0),angVelB(0,0,0); - if (rb0) - angVelA = rb0->getAngularVelocity(); - if (rb1) - angVelB = rb1->getAngularVelocity(); - btVector3 relAngVel = angVelB-angVelA; - - if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) - { - //only a single rollingFriction per manifold - rollingFriction--; - if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) - { - relAngVel.normalize(); - applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (relAngVel.length()>0.001) - addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else - { - addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - btVector3 axis0,axis1; - btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); - applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (axis0.length()>0.001) - addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if (axis1.length()>0.001) - addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } - } -#endif //ROLLING_FRICTION ///Bullet has several options to set the friction directions - ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///By default, each contact has only a single friction direction that is recomputed automatically every frame ///based on the relative linear velocity. - ///If the relative velocity it zero, it will automatically compute a friction direction. + ///If the relative velocity is zero, it will automatically compute a friction direction. ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. @@ -685,7 +1276,34 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect /// - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) + + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + cp.m_lateralFrictionDir1.normalize(); + cp.m_lateralFrictionDir2.normalize(); + + if (rollingFriction > 0 ) + { + if (cp.m_combinedSpinningFriction>0) + { + addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal); + } + if (cp.m_combinedRollingFriction>0) + { + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + + if (cp.m_lateralFrictionDir1.length()>0.001) + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); + + if (cp.m_lateralFrictionDir2.length()>0.001) + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); + } + rollingFriction--; + } + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) {/* cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); @@ -709,11 +1327,12 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* } else */ { - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { @@ -724,16 +1343,16 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) { - cp.m_lateralFrictionInitialized = true; + cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; } } } else { - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_frictionCFM); //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); //todo: @@ -809,6 +1428,7 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS } #endif + void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime) { #if 1 @@ -867,27 +1487,12 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv if (c.m_multiBodyA) { - - if(c.m_multiBodyA->isMultiDof()) - { - c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); - } - else - { - c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); - } + c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse); } if (c.m_multiBodyB) { - if(c.m_multiBodyB->isMultiDof()) - { - c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); - } - else - { - c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); - } + c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse); } #endif diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h similarity index 72% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 321ee4231a34..29f484e1d8df 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -36,6 +36,7 @@ ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpu btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; + btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints; btMultiBodyJacobianData m_data; @@ -43,12 +44,19 @@ ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpu btMultiBodyConstraint** m_tmpMultiBodyConstraints; int m_tmpNumMultiBodyConstraints; - void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); + btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); + + //solve 2 friction directions and clamp against the implicit friction cone + btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB); void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); - btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); + btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, btScalar* jacA,btScalar* jacB, @@ -60,6 +68,15 @@ ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpu btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); + + //either rolling or spinning friction + void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, + const btVector3& contactNormal, + btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + const btContactSolverInfo& infoGlobal, + btScalar& relaxation, + bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); void convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp similarity index 91% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index a9c0b33b3a3c..a2be068ed5f3 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -24,7 +24,7 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" -void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask) +void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, int group, int mask) { m_multiBodies.push_back(body); @@ -332,10 +332,10 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: } } - if (m_solverInfo->m_minimumSolverBatchSize<=1) - { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else + //if (m_solverInfo->m_minimumSolverBatchSize<=1) + //{ + // m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); + //} else { for (i=0;i world_to_local; - btAlignedObjectArray local_origin; for (int b=0;bforwardKinematics(world_to_local,local_origin); + bod->forwardKinematics(m_scratch_world_to_local,m_scratch_local_origin); } } void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { forwardKinematics(); - btAlignedObjectArray scratch_r; - btAlignedObjectArray scratch_v; - btAlignedObjectArray scratch_m; BT_PROFILE("solveConstraints"); + clearMultiBodyConstraintForces(); + m_sortedConstraints.resize( m_constraints.size()); int i; for (i=0;igetNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) - scratch_v.resize(bod->getNumLinks()+1); - scratch_m.resize(bod->getNumLinks()+1); + m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks()+1); + m_scratch_m.resize(bod->getNumLinks()+1); bod->addBaseForce(m_gravity * bod->getBaseMass()); @@ -500,15 +497,15 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) if (!isSleeping) { //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) - scratch_v.resize(bod->getNumLinks()+1); - scratch_m.resize(bod->getNumLinks()+1); + m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks()+1); + m_scratch_m.resize(bod->getNumLinks()+1); bool doNotUpdatePos = false; { if(!bod->isUsingRK4Integration()) { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m); } else { @@ -594,9 +591,9 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // btScalar h = solverInfo.m_timeStep; - #define output &scratch_r[bod->getNumDofs()] + #define output &m_scratch_r[bod->getNumDofs()] //calc qdd0 from: q0 & qd0 - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); pCopy(output, scratch_qdd0, 0, numDofs); //calc q1 = q0 + h/2 * qd0 pResetQx(); @@ -606,7 +603,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd1 from: q1 & qd1 pCopyToVelocityVector(bod, scratch_qd1); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); pCopy(output, scratch_qdd1, 0, numDofs); //calc q2 = q0 + h/2 * qd1 pResetQx(); @@ -616,7 +613,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd2 from: q2 & qd2 pCopyToVelocityVector(bod, scratch_qd2); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); pCopy(output, scratch_qdd2, 0, numDofs); //calc q3 = q0 + h * qd2 pResetQx(); @@ -626,7 +623,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) // //calc qdd3 from: q3 & qd3 pCopyToVelocityVector(bod, scratch_qd3); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., scratch_r, scratch_v, scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m); pCopy(output, scratch_qdd3, 0, numDofs); // @@ -661,7 +658,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { for(int link = 0; link < bod->getNumLinks(); ++link) bod->getLink(link).updateCacheMultiDof(); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, scratch_r, scratch_v, scratch_m); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m); } } @@ -674,7 +671,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } } - clearMultiBodyConstraintForces(); + m_solverMultiBodyIslandCallback->processConstraints(); @@ -701,16 +698,16 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) if (!isSleeping) { //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) - scratch_v.resize(bod->getNumLinks()+1); - scratch_m.resize(bod->getNumLinks()+1); + m_scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks()+1); + m_scratch_m.resize(bod->getNumLinks()+1); { if(!bod->isUsingRK4Integration()) { bool isConstraintPass = true; - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass); } } } @@ -732,9 +729,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { BT_PROFILE("btMultiBody stepPositions"); //integrate and update the Featherstone hierarchies - btAlignedObjectArray world_to_local; - btAlignedObjectArray local_origin; - + for (int b=0;bupdateCollisionObjectWorldTransforms(world_to_local,local_origin); + bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local,m_scratch_local_origin); } else { @@ -805,6 +800,8 @@ void btMultiBodyDynamicsWorld::debugDrawWorld() { BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld"); + btDiscreteDynamicsWorld::debugDrawWorld(); + bool drawConstraints = false; if (getDebugDrawer()) { @@ -818,8 +815,6 @@ void btMultiBodyDynamicsWorld::debugDrawWorld() { BT_PROFILE("btMultiBody debugDrawWorld"); - btAlignedObjectArray world_to_local1; - btAlignedObjectArray local_origin1; for (int c=0;cforwardKinematics(world_to_local1,local_origin1); - - getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1); - + bod->forwardKinematics(m_scratch_world_to_local1,m_scratch_local_origin1); + + if (mode & btIDebugDraw::DBG_DrawFrames) + { + getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1); + } for (int m = 0; mgetNumLinks(); m++) { const btTransform& tr = bod->getLink(m).m_cachedWorldTransform; - - getDebugDrawer()->drawTransform(tr, 0.1); - + if (mode & btIDebugDraw::DBG_DrawFrames) + { + getDebugDrawer()->drawTransform(tr, 0.1); + } //draw the joint axis if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute) { - btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec); + btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec)*0.1; btVector4 color(0,0,0,1);//1,1,1); btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); @@ -854,7 +852,7 @@ void btMultiBodyDynamicsWorld::debugDrawWorld() } if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed) { - btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec); + btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec)*0.1; btVector4 color(0,0,0,1);//1,1,1); btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); @@ -863,7 +861,7 @@ void btMultiBodyDynamicsWorld::debugDrawWorld() } if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic) { - btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec); + btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec)*0.1; btVector4 color(0,0,0,1);//1,1,1); btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); @@ -876,7 +874,7 @@ void btMultiBodyDynamicsWorld::debugDrawWorld() } } - btDiscreteDynamicsWorld::debugDrawWorld(); + } @@ -926,7 +924,7 @@ void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces() void btMultiBodyDynamicsWorld::clearMultiBodyForces() { { - BT_PROFILE("clearMultiBodyForces"); + // BT_PROFILE("clearMultiBodyForces"); for (int i=0;im_multiBodies.size();i++) { btMultiBody* bod = m_multiBodies[i]; @@ -977,6 +975,8 @@ void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer) serializeCollisionObjects(serializer); + serializeContactManifolds(serializer); + serializer->finishSerialization(); } @@ -995,4 +995,17 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) } } + //serialize all multibody links (collision objects) + for (i=0;igetInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + int len = colObj->calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(len,1); + const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_MB_LINKCOLLIDER_CODE,colObj); + } + } + } \ No newline at end of file diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h similarity index 84% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 03ef3335c22c..c0c132bbbab8 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -36,6 +36,16 @@ class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; + //cached data to avoid memory allocations + btAlignedObjectArray m_scratch_world_to_local; + btAlignedObjectArray m_scratch_local_origin; + btAlignedObjectArray m_scratch_world_to_local1; + btAlignedObjectArray m_scratch_local_origin1; + btAlignedObjectArray m_scratch_r; + btAlignedObjectArray m_scratch_v; + btAlignedObjectArray m_scratch_m; + + virtual void calculateSimulationIslands(); virtual void updateActivationState(btScalar timeStep); virtual void solveConstraints(btContactSolverInfo& solverInfo); @@ -48,7 +58,7 @@ class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld virtual ~btMultiBodyDynamicsWorld (); - virtual void addMultiBody(btMultiBody* body, short group= btBroadphaseProxy::DefaultFilter, short mask=btBroadphaseProxy::AllFilter); + virtual void addMultiBody(btMultiBody* body, int group= btBroadphaseProxy::DefaultFilter, int mask=btBroadphaseProxy::AllFilter); virtual void removeMultiBody(btMultiBody* body); @@ -62,6 +72,11 @@ class btMultiBodyDynamicsWorld : public btDiscreteDynamicsWorld return m_multiBodies[mbIndex]; } + const btMultiBody* getMultiBody(int mbIndex) const + { + return m_multiBodies[mbIndex]; + } + virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); virtual int getNumMultiBodyConstraints() const diff --git a/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp new file mode 100644 index 000000000000..339b3800c1ae --- /dev/null +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp @@ -0,0 +1,211 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 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 written by Erwin Coumans + +#include "btMultiBodyFixedConstraint.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" +#include "LinearMath/btIDebugDraw.h" + +#define BTMBFIXEDCONSTRAINT_DIM 6 + +btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) + :btMultiBodyConstraint(body,0,link,-1,BTMBFIXEDCONSTRAINT_DIM,false), + m_rigidBodyA(0), + m_rigidBodyB(bodyB), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB) +{ + m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses +} + +btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) + :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBFIXEDCONSTRAINT_DIM,false), + m_rigidBodyA(0), + m_rigidBodyB(0), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB) +{ + m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses +} + +void btMultiBodyFixedConstraint::finalizeMultiDof() +{ + //not implemented yet + btAssert(0); +} + +btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint() +{ +} + + +int btMultiBodyFixedConstraint::getIslandIdA() const +{ + if (m_rigidBodyA) + return m_rigidBodyA->getIslandTag(); + + if (m_bodyA) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;igetNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodyFixedConstraint::getIslandIdB() const +{ + if (m_rigidBodyB) + return m_rigidBodyB->getIslandTag(); + if (m_bodyB) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;igetNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + } + return -1; +} + +void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal) +{ + int numDim = BTMBFIXEDCONSTRAINT_DIM; + for (int i=0;igetCompanionId(); + pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; + frameAworld = frameAworld.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation()); + + } else + { + if (m_bodyA) { + pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld); + } + } + btVector3 pivotBworld = m_pivotInB; + btMatrix3x3 frameBworld = m_frameInB; + if (m_rigidBodyB) + { + constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); + pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; + frameBworld = frameBworld.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation()); + + } else + { + if (m_bodyB) { + pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld); + } + } + + btMatrix3x3 relRot = frameAworld.inverse()*frameBworld; + btVector3 angleDiff; + btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff); + + btVector3 constraintNormalLin(0,0,0); + btVector3 constraintNormalAng(0,0,0); + btScalar posError = 0.0; + if (i < 3) { + constraintNormalLin[i] = 1; + posError = (pivotAworld-pivotBworld).dot(constraintNormalLin); + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse + ); + } + else { //i>=3 + constraintNormalAng = frameAworld.getColumn(i%3); + posError = angleDiff[i%3]; + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse, true + ); + } + } +} + +void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer) +{ + btTransform tr; + tr.setIdentity(); + + if (m_rigidBodyA) + { + btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyA) + { + btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + tr.setOrigin(pivotAworld); + drawer->drawTransform(tr, 0.1); + } + if (m_rigidBodyB) + { + // that ideally should draw the same frame + btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyB) + { + btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + tr.setOrigin(pivotBworld); + drawer->drawTransform(tr, 0.1); + } +} diff --git a/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h new file mode 100644 index 000000000000..036025136ed8 --- /dev/null +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h @@ -0,0 +1,94 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 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 written by Erwin Coumans + +#ifndef BT_MULTIBODY_FIXED_CONSTRAINT_H +#define BT_MULTIBODY_FIXED_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodyFixedConstraint : public btMultiBodyConstraint +{ +protected: + + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btMatrix3x3 m_frameInA; + btMatrix3x3 m_frameInB; + +public: + + btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + + virtual ~btMultiBodyFixedConstraint(); + + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + virtual void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + const btMatrix3x3& getFrameInA() const + { + return m_frameInA; + } + + void setFrameInA(const btMatrix3x3& frameInA) + { + m_frameInA = frameInA; + } + + const btMatrix3x3& getFrameInB() const + { + return m_frameInB; + } + + virtual void setFrameInB(const btMatrix3x3& frameInB) + { + m_frameInB = frameInB; + } + + virtual void debugDraw(class btIDebugDraw* drawer); + +}; + +#endif //BT_MULTIBODY_FIXED_CONSTRAINT_H diff --git a/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp new file mode 100644 index 000000000000..b54a228ba62a --- /dev/null +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -0,0 +1,188 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 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 written by Erwin Coumans + +#include "btMultiBodyGearConstraint.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) + :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false), + m_gearRatio(1), + m_gearAuxLink(-1), + m_erp(0), + m_relativePositionTarget(0) +{ + +} + +void btMultiBodyGearConstraint::finalizeMultiDof() +{ + + allocateJacobiansMultiDof(); + + m_numDofsFinalized = m_jacSizeBoth; +} + +btMultiBodyGearConstraint::~btMultiBodyGearConstraint() +{ +} + + +int btMultiBodyGearConstraint::getIslandIdA() const +{ + + if (m_bodyA) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;igetNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodyGearConstraint::getIslandIdB() const +{ + if (m_bodyB) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;igetNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + } + return -1; +} + + +void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + + if (m_numDofsFinalized != m_jacSizeBoth) + { + finalizeMultiDof(); + } + + //don't crash + if (m_numDofsFinalized != m_jacSizeBoth) + return; + + + if (m_maxAppliedImpulse==0.f) + return; + + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + int linkDoF = 0; + unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); + unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF); + + // row 0: the lower bound + jacobianA(0)[offsetA] = 1; + jacobianB(0)[offsetB] = m_gearRatio; + + btScalar posError = 0; + const btVector3 dummy(0, 0, 0); + + btScalar kp = 1; + btScalar kd = 1; + int numRows = getNumRows(); + + for (int row=0;rowgetJointPosMultiDof(m_linkA)[dof]; + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; + btScalar auxVel = 0; + + if (m_gearAuxLink>=0) + { + auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof]; + } + currentVelocity += auxVel; + if (m_erp!=0) + { + btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; + if (m_gearAuxLink >= 0) + { + currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof]; + } + btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof]; + btScalar diff = currentPositionB+currentPositionA; + btScalar desiredPositionDiff = this->m_relativePositionTarget; + posError = -m_erp*(desiredPositionDiff - diff); + } + + btScalar desiredRelativeVelocity = auxVel; + + fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity); + + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; + { + //expect either prismatic or revolute joint type for now + btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); + switch (m_bodyA->getLink(m_linkA).m_jointType) + { + case btMultibodyLink::eRevolute: + { + constraintRow.m_contactNormal1.setZero(); + constraintRow.m_contactNormal2.setZero(); + btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); + constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld; + constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld; + + break; + } + case btMultibodyLink::ePrismatic: + { + btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); + constraintRow.m_contactNormal1=prismaticAxisInWorld; + constraintRow.m_contactNormal2=-prismaticAxisInWorld; + constraintRow.m_relpos1CrossNormal.setZero(); + constraintRow.m_relpos2CrossNormal.setZero(); + break; + } + default: + { + btAssert(0); + } + }; + + } + + } + +} + diff --git a/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h new file mode 100644 index 000000000000..0115de6241c3 --- /dev/null +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h @@ -0,0 +1,117 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 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 written by Erwin Coumans + +#ifndef BT_MULTIBODY_GEAR_CONSTRAINT_H +#define BT_MULTIBODY_GEAR_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodyGearConstraint : public btMultiBodyConstraint +{ +protected: + + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btMatrix3x3 m_frameInA; + btMatrix3x3 m_frameInB; + btScalar m_gearRatio; + int m_gearAuxLink; + btScalar m_erp; + btScalar m_relativePositionTarget; + +public: + + //btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB); + + virtual ~btMultiBodyGearConstraint(); + + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + virtual void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + const btMatrix3x3& getFrameInA() const + { + return m_frameInA; + } + + void setFrameInA(const btMatrix3x3& frameInA) + { + m_frameInA = frameInA; + } + + const btMatrix3x3& getFrameInB() const + { + return m_frameInB; + } + + virtual void setFrameInB(const btMatrix3x3& frameInB) + { + m_frameInB = frameInB; + } + + virtual void debugDraw(class btIDebugDraw* drawer) + { + //todo(erwincoumans) + } + + virtual void setGearRatio(btScalar gearRatio) + { + m_gearRatio = gearRatio; + } + virtual void setGearAuxLink(int gearAuxLink) + { + m_gearAuxLink = gearAuxLink; + } + virtual void setRelativePositionTarget(btScalar relPosTarget) + { + m_relativePositionTarget = relPosTarget; + } + virtual void setErp(btScalar erp) + { + m_erp = erp; + } +}; + +#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp similarity index 96% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 3f05aa4d5fac..6d173b66a1de 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -110,7 +110,13 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint for (int row=0;row0) + { + continue; + } btScalar direction = row? -1 : 1; btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); @@ -122,7 +128,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint const btScalar posError = 0; //why assume it's zero? const btVector3 dummy(0, 0, 0); - btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse); + btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse); { //expect either prismatic or revolute joint type for now @@ -158,7 +164,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint } { - btScalar penetration = getPosition(row); + btScalar positionalError = 0.f; btScalar velocityError = - rel_vel;// * damping; btScalar erp = infoGlobal.m_erp2; diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp similarity index 80% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 062d19accaa6..5ec9f43f0371 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -1,157 +1,186 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 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 written by Erwin Coumans - -#include "btMultiBodyJointMotor.h" -#include "btMultiBody.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - - -btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) - :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true), - m_desiredVelocity(desiredVelocity) -{ - - m_maxAppliedImpulse = maxMotorImpulse; - // the data.m_jacobians never change, so may as well - // initialize them here - - -} - -void btMultiBodyJointMotor::finalizeMultiDof() -{ - allocateJacobiansMultiDof(); - // note: we rely on the fact that data.m_jacobians are - // always initialized to zero by the Constraint ctor - int linkDoF = 0; - unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); - - // row 0: the lower bound - // row 0: the lower bound - jacobianA(0)[offset] = 1; - - m_numDofsFinalized = m_jacSizeBoth; -} - -btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse) - //:btMultiBodyConstraint(body,0,link,-1,1,true), - :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true), - m_desiredVelocity(desiredVelocity) -{ - btAssert(linkDoF < body->getLink(link).m_dofCount); - - m_maxAppliedImpulse = maxMotorImpulse; - -} -btMultiBodyJointMotor::~btMultiBodyJointMotor() -{ -} - -int btMultiBodyJointMotor::getIslandIdA() const -{ - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - return -1; -} - -int btMultiBodyJointMotor::getIslandIdB() const -{ - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->getIslandTag(); - } - return -1; -} - - -void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) -{ - // only positions need to be updated -- data.m_jacobians and force - // directions were set in the ctor and never change. - - if (m_numDofsFinalized != m_jacSizeBoth) - { - finalizeMultiDof(); - } - - //don't crash - if (m_numDofsFinalized != m_jacSizeBoth) - return; - - const btScalar posError = 0; - const btVector3 dummy(0, 0, 0); - - for (int row=0;rowgetLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); - switch (m_bodyA->getLink(m_linkA).m_jointType) - { - case btMultibodyLink::eRevolute: - { - constraintRow.m_contactNormal1.setZero(); - constraintRow.m_contactNormal2.setZero(); - btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); - constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld; - constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld; - - break; - } - case btMultibodyLink::ePrismatic: - { - btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); - constraintRow.m_contactNormal1=prismaticAxisInWorld; - constraintRow.m_contactNormal2=-prismaticAxisInWorld; - constraintRow.m_relpos1CrossNormal.setZero(); - constraintRow.m_relpos2CrossNormal.setZero(); - - break; - } - default: - { - btAssert(0); - } - }; - - } - - } - -} - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 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 written by Erwin Coumans + +#include "btMultiBodyJointMotor.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + + +btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) + :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true), + m_desiredVelocity(desiredVelocity), + m_desiredPosition(0), + m_kd(1.), + m_kp(0), + m_erp(1), + m_rhsClamp(SIMD_INFINITY) +{ + + m_maxAppliedImpulse = maxMotorImpulse; + // the data.m_jacobians never change, so may as well + // initialize them here + + +} + +void btMultiBodyJointMotor::finalizeMultiDof() +{ + allocateJacobiansMultiDof(); + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + int linkDoF = 0; + unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); + + // row 0: the lower bound + // row 0: the lower bound + jacobianA(0)[offset] = 1; + + m_numDofsFinalized = m_jacSizeBoth; +} + +btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse) + //:btMultiBodyConstraint(body,0,link,-1,1,true), + :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true), + m_desiredVelocity(desiredVelocity), + m_desiredPosition(0), + m_kd(1.), + m_kp(0), + m_erp(1), + m_rhsClamp(SIMD_INFINITY) +{ + btAssert(linkDoF < body->getLink(link).m_dofCount); + + m_maxAppliedImpulse = maxMotorImpulse; + +} +btMultiBodyJointMotor::~btMultiBodyJointMotor() +{ +} + +int btMultiBodyJointMotor::getIslandIdA() const +{ + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;igetNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + return -1; +} + +int btMultiBodyJointMotor::getIslandIdB() const +{ + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;igetNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + return -1; +} + + +void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + + if (m_numDofsFinalized != m_jacSizeBoth) + { + finalizeMultiDof(); + } + + //don't crash + if (m_numDofsFinalized != m_jacSizeBoth) + return; + + if (m_maxAppliedImpulse==0.f) + return; + + const btScalar posError = 0; + const btVector3 dummy(0, 0, 0); + + for (int row=0;rowgetJointPosMultiDof(m_linkA)[dof]; + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; + btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; + + btScalar velocityError = (m_desiredVelocity - currentVelocity); + btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError; + if (rhs>m_rhsClamp) + { + rhs=m_rhsClamp; + } + if (rhs<-m_rhsClamp) + { + rhs=-m_rhsClamp; + } + + + fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,rhs); + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; + { + //expect either prismatic or revolute joint type for now + btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); + switch (m_bodyA->getLink(m_linkA).m_jointType) + { + case btMultibodyLink::eRevolute: + { + constraintRow.m_contactNormal1.setZero(); + constraintRow.m_contactNormal2.setZero(); + btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); + constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld; + constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld; + + break; + } + case btMultibodyLink::ePrismatic: + { + btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); + constraintRow.m_contactNormal1=prismaticAxisInWorld; + constraintRow.m_contactNormal2=-prismaticAxisInWorld; + constraintRow.m_relpos1CrossNormal.setZero(); + constraintRow.m_relpos2CrossNormal.setZero(); + + break; + } + default: + { + btAssert(0); + } + }; + + } + + } + +} + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h similarity index 75% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index 011aadcfa4b7..7cf57311afb6 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -1,57 +1,81 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 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 written by Erwin Coumans - -#ifndef BT_MULTIBODY_JOINT_MOTOR_H -#define BT_MULTIBODY_JOINT_MOTOR_H - -#include "btMultiBodyConstraint.h" -struct btSolverInfo; - -class btMultiBodyJointMotor : public btMultiBodyConstraint -{ -protected: - - - btScalar m_desiredVelocity; - -public: - - btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse); - btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse); - virtual ~btMultiBodyJointMotor(); - virtual void finalizeMultiDof(); - - virtual int getIslandIdA() const; - virtual int getIslandIdB() const; - - virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal); - - virtual void setVelocityTarget(btScalar velTarget) - { - m_desiredVelocity = velTarget; - } - - virtual void debugDraw(class btIDebugDraw* drawer) - { - //todo(erwincoumans) - } -}; - -#endif //BT_MULTIBODY_JOINT_MOTOR_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 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 written by Erwin Coumans + +#ifndef BT_MULTIBODY_JOINT_MOTOR_H +#define BT_MULTIBODY_JOINT_MOTOR_H + +#include "btMultiBodyConstraint.h" +struct btSolverInfo; + +class btMultiBodyJointMotor : public btMultiBodyConstraint +{ +protected: + + btScalar m_desiredVelocity; + btScalar m_desiredPosition; + btScalar m_kd; + btScalar m_kp; + btScalar m_erp; + btScalar m_rhsClamp;//maximum error + + +public: + + btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse); + btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse); + virtual ~btMultiBodyJointMotor(); + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f) + { + m_desiredVelocity = velTarget; + m_kd = kd; + } + + virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f) + { + m_desiredPosition = posTarget; + m_kp = kp; + } + + virtual void setErp(btScalar erp) + { + m_erp = erp; + } + virtual btScalar getErp() const + { + return m_erp; + } + virtual void setRhsClamp(btScalar rhsClamp) + { + m_rhsClamp = rhsClamp; + } + virtual void debugDraw(class btIDebugDraw* drawer) + { + //todo(erwincoumans) + } +}; + +#endif //BT_MULTIBODY_JOINT_MOTOR_H + diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h similarity index 80% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 668e44439045..13e906cd65db 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -1,219 +1,246 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 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. -*/ - -#ifndef BT_MULTIBODY_LINK_H -#define BT_MULTIBODY_LINK_H - -#include "LinearMath/btQuaternion.h" -#include "LinearMath/btVector3.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - -enum btMultiBodyLinkFlags -{ - BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1 -}; - -//both defines are now permanently enabled -#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS -#define TEST_SPATIAL_ALGEBRA_LAYER - -// -// Various spatial helper functions -// - -//namespace { - - -#include "LinearMath/btSpatialAlgebra.h" - -//} - -// -// Link struct -// - -struct btMultibodyLink -{ - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btScalar m_mass; // mass of link - btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal) - - int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link. - - btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant. - - btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. - //this is set to zero for planar joint (see also m_eVector comment) - - // m_eVector is constant, but depends on the joint type: - // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame. - // planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.) - // todo: fix the planar so it is consistent with the other joints - - btVector3 m_eVector; - - btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity; - - enum eFeatherstoneJointType - { - eRevolute = 0, - ePrismatic = 1, - eSpherical = 2, - ePlanar = 3, - eFixed = 4, - eInvalid - }; - - - - // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant. - // for prismatic: m_axesTop[0] = zero; - // m_axesBottom[0] = unit vector along the joint axis. - // for revolute: m_axesTop[0] = unit vector along the rotation axis (u); - // m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint) - // - // for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes) - // m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint) - // - // for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion - // m_axesTop[1][2] = zero - // m_axesBottom[0] = zero - // m_axesBottom[1][2] = unit vectors along the translational axes on that plane - btSpatialMotionVector m_axes[6]; - void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; } - void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; } - void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); } - void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); } - const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; } - const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; } - - int m_dofOffset, m_cfgOffset; - - btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame - btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame. - - btVector3 m_appliedForce; // In WORLD frame - btVector3 m_appliedTorque; // In WORLD frame - -btVector3 m_appliedConstraintForce; // In WORLD frame - btVector3 m_appliedConstraintTorque; // In WORLD frame - - btScalar m_jointPos[7]; - - //m_jointTorque is the joint torque applied by the user using 'addJointTorque'. - //It gets set to zero after each internal stepSimulation call - btScalar m_jointTorque[6]; - - class btMultiBodyLinkCollider* m_collider; - int m_flags; - - - int m_dofCount, m_posVarCount; //redundant but handy - - eFeatherstoneJointType m_jointType; - - struct btMultiBodyJointFeedback* m_jointFeedback; - - btTransform m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics - - const char* m_linkName;//m_linkName memory needs to be managed by the developer/user! - const char* m_jointName;//m_jointName memory needs to be managed by the developer/user! - - // ctor: set some sensible defaults - btMultibodyLink() - : m_mass(1), - m_parent(-1), - m_zeroRotParentToThis(0, 0, 0, 1), - m_cachedRotParentToThis(0, 0, 0, 1), - m_collider(0), - m_flags(0), - m_dofCount(0), - m_posVarCount(0), - m_jointType(btMultibodyLink::eInvalid), - m_jointFeedback(0), - m_linkName(0), - m_jointName(0) - { - - m_inertiaLocal.setValue(1, 1, 1); - setAxisTop(0, 0., 0., 0.); - setAxisBottom(0, 1., 0., 0.); - m_dVector.setValue(0, 0, 0); - m_eVector.setValue(0, 0, 0); - m_cachedRVector.setValue(0, 0, 0); - m_appliedForce.setValue( 0, 0, 0); - m_appliedTorque.setValue(0, 0, 0); - // - m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f; - m_jointPos[3] = 1.f; //"quat.w" - m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f; - m_cachedWorldTransform.setIdentity(); - } - - // routine to update m_cachedRotParentToThis and m_cachedRVector - void updateCacheMultiDof(btScalar *pq = 0) - { - btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); - - switch(m_jointType) - { - case eRevolute: - { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); - - break; - } - case ePrismatic: - { - // m_cachedRotParentToThis never changes, so no need to update - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0); - - break; - } - case eSpherical: - { - m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); - - break; - } - case ePlanar: - { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector); - - break; - } - case eFixed: - { - m_cachedRotParentToThis = m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); - - break; - } - default: - { - //invalid type - btAssert(0); - } - } - } -}; - - -#endif //BT_MULTIBODY_LINK_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 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. +*/ + +#ifndef BT_MULTIBODY_LINK_H +#define BT_MULTIBODY_LINK_H + +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btVector3.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +enum btMultiBodyLinkFlags +{ + BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1, + BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2, +}; + +//both defines are now permanently enabled +#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS +#define TEST_SPATIAL_ALGEBRA_LAYER + +// +// Various spatial helper functions +// + +//namespace { + + +#include "LinearMath/btSpatialAlgebra.h" + +//} + +// +// Link struct +// + +struct btMultibodyLink +{ + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btScalar m_mass; // mass of link + btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal) + + int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link. + + btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant. + + btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. + //this is set to zero for planar joint (see also m_eVector comment) + + // m_eVector is constant, but depends on the joint type: + // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame. + // planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.) + // todo: fix the planar so it is consistent with the other joints + + btVector3 m_eVector; + + btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity; + + enum eFeatherstoneJointType + { + eRevolute = 0, + ePrismatic = 1, + eSpherical = 2, + ePlanar = 3, + eFixed = 4, + eInvalid + }; + + + + // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant. + // for prismatic: m_axesTop[0] = zero; + // m_axesBottom[0] = unit vector along the joint axis. + // for revolute: m_axesTop[0] = unit vector along the rotation axis (u); + // m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint) + // + // for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes) + // m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint) + // + // for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion + // m_axesTop[1][2] = zero + // m_axesBottom[0] = zero + // m_axesBottom[1][2] = unit vectors along the translational axes on that plane + btSpatialMotionVector m_axes[6]; + void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; } + void setAxisBottom(int dof, const btVector3 &axis) + { + m_axes[dof].m_bottomVec = axis; + } + void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) + { + m_axes[dof].m_topVec.setValue(x, y, z); + } + void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) + { + m_axes[dof].m_bottomVec.setValue(x, y, z); + } + const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; } + const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; } + + int m_dofOffset, m_cfgOffset; + + btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame + btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame. + + btVector3 m_appliedForce; // In WORLD frame + btVector3 m_appliedTorque; // In WORLD frame + +btVector3 m_appliedConstraintForce; // In WORLD frame + btVector3 m_appliedConstraintTorque; // In WORLD frame + + btScalar m_jointPos[7]; + + //m_jointTorque is the joint torque applied by the user using 'addJointTorque'. + //It gets set to zero after each internal stepSimulation call + btScalar m_jointTorque[6]; + + class btMultiBodyLinkCollider* m_collider; + int m_flags; + + + int m_dofCount, m_posVarCount; //redundant but handy + + eFeatherstoneJointType m_jointType; + + struct btMultiBodyJointFeedback* m_jointFeedback; + + btTransform m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics + + const char* m_linkName;//m_linkName memory needs to be managed by the developer/user! + const char* m_jointName;//m_jointName memory needs to be managed by the developer/user! + const void* m_userPtr;//m_userPtr ptr needs to be managed by the developer/user! + + btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping. + btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor. + btScalar m_jointLowerLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader. + btScalar m_jointUpperLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader. + btScalar m_jointMaxForce; //todo: implement this internally. It is unused for now, it is set by a URDF loader. + btScalar m_jointMaxVelocity;//todo: implement this internally. It is unused for now, it is set by a URDF loader. + + // ctor: set some sensible defaults + btMultibodyLink() + : m_mass(1), + m_parent(-1), + m_zeroRotParentToThis(0, 0, 0, 1), + m_cachedRotParentToThis(0, 0, 0, 1), + m_collider(0), + m_flags(0), + m_dofCount(0), + m_posVarCount(0), + m_jointType(btMultibodyLink::eInvalid), + m_jointFeedback(0), + m_linkName(0), + m_jointName(0), + m_userPtr(0), + m_jointDamping(0), + m_jointFriction(0), + m_jointLowerLimit(0), + m_jointUpperLimit(0), + m_jointMaxForce(0), + m_jointMaxVelocity(0) + { + + m_inertiaLocal.setValue(1, 1, 1); + setAxisTop(0, 0., 0., 0.); + setAxisBottom(0, 1., 0., 0.); + m_dVector.setValue(0, 0, 0); + m_eVector.setValue(0, 0, 0); + m_cachedRVector.setValue(0, 0, 0); + m_appliedForce.setValue( 0, 0, 0); + m_appliedTorque.setValue(0, 0, 0); + m_appliedConstraintForce.setValue(0,0,0); + m_appliedConstraintTorque.setValue(0,0,0); + // + m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f; + m_jointPos[3] = 1.f; //"quat.w" + m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f; + m_cachedWorldTransform.setIdentity(); + } + + // routine to update m_cachedRotParentToThis and m_cachedRVector + void updateCacheMultiDof(btScalar *pq = 0) + { + btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); + + switch(m_jointType) + { + case eRevolute: + { + m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; + m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); + + break; + } + case ePrismatic: + { + // m_cachedRotParentToThis never changes, so no need to update + m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0); + + break; + } + case eSpherical: + { + m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; + m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); + + break; + } + case ePlanar: + { + m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; + m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector); + + break; + } + case eFixed: + { + m_cachedRotParentToThis = m_zeroRotParentToThis; + m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); + + break; + } + default: + { + //invalid type + btAssert(0); + } + } + } +}; + + +#endif //BT_MULTIBODY_LINK_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h similarity index 52% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index 5080ea874541..7092e62b5ae3 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -4,8 +4,8 @@ Copyright (c) 2013 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, +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. @@ -19,6 +19,16 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "btMultiBody.h" +#include "LinearMath/btSerializer.h" + +#ifdef BT_USE_DOUBLE_PRECISION +#define btMultiBodyLinkColliderData btMultiBodyLinkColliderDoubleData +#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderDoubleData" +#else +#define btMultiBodyLinkColliderData btMultiBodyLinkColliderFloatData +#define btMultiBodyLinkColliderDataName "btMultiBodyLinkColliderFloatData" +#endif + class btMultiBodyLinkCollider : public btCollisionObject { @@ -74,19 +84,94 @@ class btMultiBodyLinkCollider : public btCollisionObject if (m_link>=0) { const btMultibodyLink& link = m_multiBody->getLink(this->m_link); - if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.m_parent == other->m_link) - return false; + if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) + { + int parent_of_this = m_link; + while (1) + { + if (parent_of_this==-1) + break; + parent_of_this = m_multiBody->getLink(parent_of_this).m_parent; + if (parent_of_this==other->m_link) + { + return false; + } + } + } + else if (link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) + { + if ( link.m_parent == other->m_link) + return false; + } + } - + if (other->m_link>=0) { const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link); - if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.m_parent == this->m_link) - return false; + if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION) + { + int parent_of_other = other->m_link; + while (1) + { + if (parent_of_other==-1) + break; + parent_of_other = m_multiBody->getLink(parent_of_other).m_parent; + if (parent_of_other==this->m_link) + return false; + } + } + else if (otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) + { + if (otherLink.m_parent == this->m_link) + return false; + } } return true; } + + virtual int calculateSerializeBufferSize() const; + + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; + +}; + + +struct btMultiBodyLinkColliderFloatData +{ + btCollisionObjectFloatData m_colObjData; + btMultiBodyFloatData *m_multiBody; + int m_link; + char m_padding[4]; }; +struct btMultiBodyLinkColliderDoubleData +{ + btCollisionObjectDoubleData m_colObjData; + btMultiBodyDoubleData *m_multiBody; + int m_link; + char m_padding[4]; +}; + +SIMD_FORCE_INLINE int btMultiBodyLinkCollider::calculateSerializeBufferSize() const +{ + return sizeof(btMultiBodyLinkColliderData); +} + +SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffer, class btSerializer* serializer) const +{ + btMultiBodyLinkColliderData* dataOut = (btMultiBodyLinkColliderData*)dataBuffer; + btCollisionObject::serialize(&dataOut->m_colObjData,serializer); + + dataOut->m_link = this->m_link; + dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody); + + // Fill padding with zeros to appease msan. + memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding)); + + return btMultiBodyLinkColliderDataName; +} + #endif //BT_FEATHERSTONE_LINK_COLLIDER_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp similarity index 95% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 12b21f74603a..3e28f80dfa4e 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -1,221 +1,221 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2013 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 written by Erwin Coumans - -#include "btMultiBodyPoint2Point.h" -#include "btMultiBodyLinkCollider.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btIDebugDraw.h" - -#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - #define BTMBP2PCONSTRAINT_DIM 3 -#else - #define BTMBP2PCONSTRAINT_DIM 6 -#endif - -btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false), - m_rigidBodyA(0), - m_rigidBodyB(bodyB), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB) -{ - m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses -} - -btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false), - m_rigidBodyA(0), - m_rigidBodyB(0), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB) -{ - m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses -} - -void btMultiBodyPoint2Point::finalizeMultiDof() -{ - //not implemented yet - btAssert(0); -} - -btMultiBodyPoint2Point::~btMultiBodyPoint2Point() -{ -} - - -int btMultiBodyPoint2Point::getIslandIdA() const -{ - if (m_rigidBodyA) - return m_rigidBodyA->getIslandTag(); - - if (m_bodyA) - { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;igetNumLinks();i++) - { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); - } - } - return -1; -} - -int btMultiBodyPoint2Point::getIslandIdB() const -{ - if (m_rigidBodyB) - return m_rigidBodyB->getIslandTag(); - if (m_bodyB) - { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;igetNumLinks();i++) - { - col = m_bodyB->getLink(i).m_collider; - if (col) - return col->getIslandTag(); - } - } - return -1; -} - - - -void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) -{ - -// int i=1; -int numDim = BTMBP2PCONSTRAINT_DIM; - for (int i=0;igetCompanionId(); - pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; - } else - { - if (m_bodyA) - pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); - } - btVector3 pivotBworld = m_pivotInB; - if (m_rigidBodyB) - { - constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); - pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; - } else - { - if (m_bodyB) - pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); - - } - - btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0; - -#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - - - fillMultiBodyConstraint(constraintRow, data, 0, 0, - contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being" - posError, - infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse - ); - //@todo: support the case of btMultiBody versus btRigidBody, - //see btPoint2PointConstraint::getInfo2NonVirtual -#else - const btVector3 dummy(0, 0, 0); - - btAssert(m_bodyA->isMultiDof()); - - btScalar* jac1 = jacobianA(i); - const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy; - const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy; - - m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); - - fillMultiBodyConstraint(constraintRow, data, jac1, 0, - dummy, dummy, dummy, //sucks but let it be this way "for the time being" - posError, - infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse - ); -#endif - } -} - -void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer) -{ - btTransform tr; - tr.setIdentity(); - - if (m_rigidBodyA) - { - btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; - tr.setOrigin(pivot); - drawer->drawTransform(tr, 0.1); - } - if (m_bodyA) - { - btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); - tr.setOrigin(pivotAworld); - drawer->drawTransform(tr, 0.1); - } - if (m_rigidBodyB) - { - // that ideally should draw the same frame - btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; - tr.setOrigin(pivot); - drawer->drawTransform(tr, 0.1); - } - if (m_bodyB) - { - btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); - tr.setOrigin(pivotBworld); - drawer->drawTransform(tr, 0.1); - } -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 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 written by Erwin Coumans + +#include "btMultiBodyPoint2Point.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btIDebugDraw.h" + +#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST + #define BTMBP2PCONSTRAINT_DIM 3 +#else + #define BTMBP2PCONSTRAINT_DIM 6 +#endif + +btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) + :btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false), + m_rigidBodyA(0), + m_rigidBodyB(bodyB), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB) +{ + m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses +} + +btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) + :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false), + m_rigidBodyA(0), + m_rigidBodyB(0), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB) +{ + m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses +} + +void btMultiBodyPoint2Point::finalizeMultiDof() +{ + //not implemented yet + btAssert(0); +} + +btMultiBodyPoint2Point::~btMultiBodyPoint2Point() +{ +} + + +int btMultiBodyPoint2Point::getIslandIdA() const +{ + if (m_rigidBodyA) + return m_rigidBodyA->getIslandTag(); + + if (m_bodyA) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;igetNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodyPoint2Point::getIslandIdB() const +{ + if (m_rigidBodyB) + return m_rigidBodyB->getIslandTag(); + if (m_bodyB) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;igetNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + } + return -1; +} + + + +void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + +// int i=1; +int numDim = BTMBP2PCONSTRAINT_DIM; + for (int i=0;igetCompanionId(); + pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; + } else + { + if (m_bodyA) + pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + } + btVector3 pivotBworld = m_pivotInB; + if (m_rigidBodyB) + { + constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); + pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; + } else + { + if (m_bodyB) + pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + + } + + btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0; + +#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST + + + fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0,0,0), + contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being" + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse + ); + //@todo: support the case of btMultiBody versus btRigidBody, + //see btPoint2PointConstraint::getInfo2NonVirtual +#else + const btVector3 dummy(0, 0, 0); + + btAssert(m_bodyA->isMultiDof()); + + btScalar* jac1 = jacobianA(i); + const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy; + const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy; + + m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + + fillMultiBodyConstraint(constraintRow, data, jac1, 0, + dummy, dummy, dummy, //sucks but let it be this way "for the time being" + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse + ); +#endif + } +} + +void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer) +{ + btTransform tr; + tr.setIdentity(); + + if (m_rigidBodyA) + { + btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyA) + { + btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + tr.setOrigin(pivotAworld); + drawer->drawTransform(tr, 0.1); + } + if (m_rigidBodyB) + { + // that ideally should draw the same frame + btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyB) + { + btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + tr.setOrigin(pivotBworld); + drawer->drawTransform(tr, 0.1); + } +} diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h similarity index 92% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index b2e219ac1590..bf39acc5b95f 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -22,7 +22,7 @@ subject to the following restrictions: //#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST -class btMultiBodyPoint2Point : public btMultiBodyConstraint +ATTRIBUTE_ALIGNED16(class) btMultiBodyPoint2Point : public btMultiBodyConstraint { protected: @@ -34,6 +34,8 @@ class btMultiBodyPoint2Point : public btMultiBodyConstraint public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB); btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB); @@ -53,11 +55,12 @@ class btMultiBodyPoint2Point : public btMultiBodyConstraint return m_pivotInB; } - void setPivotInB(const btVector3& pivotInB) + virtual void setPivotInB(const btVector3& pivotInB) { m_pivotInB = pivotInB; } + virtual void debugDraw(class btIDebugDraw* drawer); }; diff --git a/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp new file mode 100644 index 000000000000..67b106f28f80 --- /dev/null +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp @@ -0,0 +1,230 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 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 written by Erwin Coumans + +#include "btMultiBodySliderConstraint.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" +#include "LinearMath/btIDebugDraw.h" + +#define BTMBSLIDERCONSTRAINT_DIM 5 +#define EPSILON 0.000001 + +btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) + :btMultiBodyConstraint(body,0,link,-1,BTMBSLIDERCONSTRAINT_DIM,false), + m_rigidBodyA(0), + m_rigidBodyB(bodyB), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB), + m_jointAxis(jointAxis) +{ + m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses +} + +btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) + :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBSLIDERCONSTRAINT_DIM,false), + m_rigidBodyA(0), + m_rigidBodyB(0), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB), + m_jointAxis(jointAxis) +{ + m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses +} + +void btMultiBodySliderConstraint::finalizeMultiDof() +{ + //not implemented yet + btAssert(0); +} + +btMultiBodySliderConstraint::~btMultiBodySliderConstraint() +{ +} + + +int btMultiBodySliderConstraint::getIslandIdA() const +{ + if (m_rigidBodyA) + return m_rigidBodyA->getIslandTag(); + + if (m_bodyA) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;igetNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodySliderConstraint::getIslandIdB() const +{ + if (m_rigidBodyB) + return m_rigidBodyB->getIslandTag(); + if (m_bodyB) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;igetNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + } + return -1; +} + +void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal) +{ + // Convert local points back to world + btVector3 pivotAworld = m_pivotInA; + btMatrix3x3 frameAworld = m_frameInA; + btVector3 jointAxis = m_jointAxis; + if (m_rigidBodyA) + { + pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; + frameAworld = m_frameInA.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation()); + jointAxis = quatRotate(m_rigidBodyA->getOrientation(),m_jointAxis); + + } else if (m_bodyA) { + pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA); + jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis); + } + btVector3 pivotBworld = m_pivotInB; + btMatrix3x3 frameBworld = m_frameInB; + if (m_rigidBodyB) + { + pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; + frameBworld = m_frameInB.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation()); + + } else if (m_bodyB) { + pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB); + } + + btVector3 constraintAxis[2]; + for (int i = 0; i < 3; ++i) + { + constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis); + if (constraintAxis[0].safeNorm() > EPSILON) + { + constraintAxis[0] = constraintAxis[0].normalized(); + constraintAxis[1] = jointAxis.cross(constraintAxis[0]); + constraintAxis[1] = constraintAxis[1].normalized(); + break; + } + } + + btMatrix3x3 relRot = frameAworld.inverse()*frameBworld; + btVector3 angleDiff; + btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff); + + int numDim = BTMBSLIDERCONSTRAINT_DIM; + for (int i=0;igetCompanionId(); + } + if (m_rigidBodyB) + { + constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); + } + + btVector3 constraintNormalLin(0,0,0); + btVector3 constraintNormalAng(0,0,0); + btScalar posError = 0.0; + if (i < 2) { + constraintNormalLin = constraintAxis[i]; + posError = (pivotAworld-pivotBworld).dot(constraintNormalLin); + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse + ); + } + else { //i>=2 + constraintNormalAng = frameAworld.getColumn(i%3); + posError = angleDiff[i%3]; + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse, true + ); + } + } +} + +void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer) +{ + btTransform tr; + tr.setIdentity(); + + if (m_rigidBodyA) + { + btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyA) + { + btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + tr.setOrigin(pivotAworld); + drawer->drawTransform(tr, 0.1); + } + if (m_rigidBodyB) + { + // that ideally should draw the same frame + btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyB) + { + btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + tr.setOrigin(pivotBworld); + drawer->drawTransform(tr, 0.1); + } +} diff --git a/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h new file mode 100644 index 000000000000..0a6cf3df12b5 --- /dev/null +++ b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h @@ -0,0 +1,105 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 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 written by Erwin Coumans + +#ifndef BT_MULTIBODY_SLIDER_CONSTRAINT_H +#define BT_MULTIBODY_SLIDER_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodySliderConstraint : public btMultiBodyConstraint +{ +protected: + + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btMatrix3x3 m_frameInA; + btMatrix3x3 m_frameInB; + btVector3 m_jointAxis; + +public: + + btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis); + btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis); + + virtual ~btMultiBodySliderConstraint(); + + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + virtual void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + const btMatrix3x3& getFrameInA() const + { + return m_frameInA; + } + + void setFrameInA(const btMatrix3x3& frameInA) + { + m_frameInA = frameInA; + } + + const btMatrix3x3& getFrameInB() const + { + return m_frameInB; + } + + virtual void setFrameInB(const btMatrix3x3& frameInB) + { + m_frameInB = frameInB; + } + + const btVector3& getJointAxis() const + { + return m_jointAxis; + } + + void setJointAxis(const btVector3& jointAxis) + { + m_jointAxis = jointAxis; + } + + virtual void debugDraw(class btIDebugDraw* drawer); + +}; + +#endif //BT_MULTIBODY_SLIDER_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/extern/bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h rename to extern/bullet/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/extern/bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp similarity index 100% rename from extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp rename to extern/bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h b/extern/bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h rename to extern/bullet/src/BulletDynamics/MLCPSolvers/btDantzigLCP.h diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h b/extern/bullet/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h rename to extern/bullet/src/BulletDynamics/MLCPSolvers/btDantzigSolver.h diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp b/extern/bullet/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp similarity index 100% rename from extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp rename to extern/bullet/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h b/extern/bullet/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h rename to extern/bullet/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h b/extern/bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h rename to extern/bullet/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/extern/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp similarity index 97% rename from extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp rename to extern/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp index 6688694a9282..8f54c5262697 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp +++ b/extern/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -22,8 +22,7 @@ subject to the following restrictions: btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver) :m_solver(solver), -m_fallback(0), -m_cfm(0.000001)//0.0000001 +m_fallback(0) { } @@ -216,12 +215,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); } - static btMatrixXu J3; + btMatrixXu& J3 = m_scratchJ3; { BT_PROFILE("J3.resize"); J3.resize(2*m,8); } - static btMatrixXu JinvM3; + btMatrixXu& JinvM3 = m_scratchJInvM3; { BT_PROFILE("JinvM3.resize/setZero"); @@ -231,7 +230,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) } int cur=0; int rowOffset = 0; - static btAlignedObjectArray ofs; + btAlignedObjectArray& ofs = m_scratchOfs; { BT_PROFILE("ofs resize"); ofs.resize(0); @@ -436,7 +435,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) // add cfm to the diagonal of m_A for ( int i=0; igetInvInertiaTensorWorld()[r][c] : 0); } - static btMatrixXu J; + btMatrixXu& J = m_scratchJ; J.resize(numConstraintRows,6*numBodies); J.setZero(); @@ -542,10 +541,10 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) } } - static btMatrixXu J_transpose; + btMatrixXu& J_transpose = m_scratchJTranspose; J_transpose= J.transpose(); - static btMatrixXu tmp; + btMatrixXu& tmp = m_scratchTmp; { { @@ -564,7 +563,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) // add cfm to the diagonal of m_A for ( int i=0; i m_allConstraintPtrArray; btMLCPSolverInterface* m_solver; int m_fallback; - btScalar m_cfm; + + /// The following scratch variables are not stateful -- contents are cleared prior to each use. + /// They are only cached here to avoid extra memory allocations and deallocations and to ensure + /// that multiple instances of the solver can be run in parallel. + btMatrixXu m_scratchJ3; + btMatrixXu m_scratchJInvM3; + btAlignedObjectArray m_scratchOfs; + btMatrixXu m_scratchMInv; + btMatrixXu m_scratchJ; + btMatrixXu m_scratchJTranspose; + btMatrixXu m_scratchTmp; virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); @@ -73,15 +83,6 @@ class btMLCPSolver : public btSequentialImpulseConstraintSolver m_fallback = num; } - btScalar getCfm() const - { - return m_cfm; - } - void setCfm(btScalar cfm) - { - m_cfm = cfm; - } - virtual btConstraintSolverType getSolverType() const { return BT_MLCP_SOLVER; diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h b/extern/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h rename to extern/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h b/extern/bullet/src/BulletDynamics/MLCPSolvers/btPATHSolver.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/MLCPSolvers/btPATHSolver.h rename to extern/bullet/src/BulletDynamics/MLCPSolvers/btPATHSolver.h diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/extern/bullet/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h similarity index 80% rename from extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h rename to extern/bullet/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h index 77cc57c6e0ee..c0b40ffd9f80 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h +++ b/extern/bullet/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h @@ -23,7 +23,18 @@ subject to the following restrictions: ///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix) class btSolveProjectedGaussSeidel : public btMLCPSolverInterface { + public: + + btScalar m_leastSquaresResidualThreshold; + btScalar m_leastSquaresResidual; + + btSolveProjectedGaussSeidel() + :m_leastSquaresResidualThreshold(0), + m_leastSquaresResidual(0) + { + } + virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true) { if (!A.rows()) @@ -36,10 +47,11 @@ class btSolveProjectedGaussSeidel : public btMLCPSolverInterface int i, j, numRows = A.rows(); - float delta; + btScalar delta; for (int k = 0; k =0) { @@ -76,6 +89,17 @@ class btSolveProjectedGaussSeidel : public btMLCPSolverInterface x[i]=lo[i]*s; if (x[i]>hi[i]*s) x[i]=hi[i]*s; + btScalar diff = x[i] - xOld; + m_leastSquaresResidual += diff*diff; + } + + btScalar eps = m_leastSquaresResidualThreshold; + if ((m_leastSquaresResidual < eps) || (k >=(numIterations-1))) + { +#ifdef VERBOSE_PRINTF_RESIDUAL + printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual,k); +#endif + break; } } return true; diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp similarity index 96% rename from extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp rename to extern/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index a7b1688469fd..f299aa34e8d4 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/extern/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -121,12 +121,19 @@ void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedT 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] - ); - + btMatrix3x3 basis2; + basis2[0][m_indexRightAxis] = -right[0]; + basis2[1][m_indexRightAxis] = -right[1]; + basis2[2][m_indexRightAxis] = -right[2]; + + basis2[0][m_indexUpAxis] = up[0]; + basis2[1][m_indexUpAxis] = up[1]; + basis2[2][m_indexUpAxis] = up[2]; + + basis2[0][m_indexForwardAxis] = fwd[0]; + basis2[1][m_indexForwardAxis] = fwd[1]; + basis2[2][m_indexForwardAxis] = fwd[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 @@ -493,8 +500,8 @@ struct btWheelContactPoint }; -btScalar calcRollingFriction(btWheelContactPoint& contactPoint); -btScalar calcRollingFriction(btWheelContactPoint& contactPoint) +btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround); +btScalar calcRollingFriction(btWheelContactPoint& contactPoint, int numWheelsOnGround) { btScalar j1=0.f; @@ -513,7 +520,7 @@ btScalar calcRollingFriction(btWheelContactPoint& contactPoint) btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel); // calculate j that moves us to zero relative velocity - j1 = -vrel * contactPoint.m_jacDiagABInv; + j1 = -vrel * contactPoint.m_jacDiagABInv/btScalar(numWheelsOnGround); btSetMin(j1, maxImpulse); btSetMax(j1, -maxImpulse); @@ -567,7 +574,7 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) const btTransform& wheelTrans = getWheelTransformWS( i ); btMatrix3x3 wheelBasis0 = wheelTrans.getBasis(); - m_axle[i] = btVector3( + m_axle[i] = -btVector3( wheelBasis0[0][m_indexRightAxis], wheelBasis0[1][m_indexRightAxis], wheelBasis0[2][m_indexRightAxis]); @@ -615,7 +622,8 @@ void btRaycastVehicle::updateFriction(btScalar timeStep) 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); + btAssert(numWheelsOnGround > 0); + rollingFriction = calcRollingFriction(contactPt, numWheelsOnGround); } } diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h similarity index 99% rename from extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h rename to extern/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h index 82d44c73e054..04656b912c7b 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/extern/bullet/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -19,7 +19,7 @@ class btDynamicsWorld; #include "btWheelInfo.h" #include "BulletDynamics/Dynamics/btActionInterface.h" -class btVehicleTuning; +//class btVehicleTuning; ///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle. class btRaycastVehicle : public btActionInterface diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h b/extern/bullet/src/BulletDynamics/Vehicle/btVehicleRaycaster.h similarity index 100% rename from extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h rename to extern/bullet/src/BulletDynamics/Vehicle/btVehicleRaycaster.h diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp b/extern/bullet/src/BulletDynamics/Vehicle/btWheelInfo.cpp similarity index 100% rename from extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp rename to extern/bullet/src/BulletDynamics/Vehicle/btWheelInfo.cpp diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h b/extern/bullet/src/BulletDynamics/Vehicle/btWheelInfo.h similarity index 99% rename from extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h rename to extern/bullet/src/BulletDynamics/Vehicle/btWheelInfo.h index f916053ecac1..f991a57b69e3 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h +++ b/extern/bullet/src/BulletDynamics/Vehicle/btWheelInfo.h @@ -79,6 +79,8 @@ struct btWheelInfo void* m_clientInfo;//can be used to store pointer to sync transforms... + btWheelInfo() {} + btWheelInfo(btWheelInfoConstructionInfo& ci) { diff --git a/extern/bullet/src/BulletDynamics/premake4.lua b/extern/bullet/src/BulletDynamics/premake4.lua new file mode 100644 index 000000000000..fdaf0513d896 --- /dev/null +++ b/extern/bullet/src/BulletDynamics/premake4.lua @@ -0,0 +1,24 @@ + project "BulletDynamics" + kind "StaticLib" + includedirs { + "..", + } + if os.is("Linux") then + buildoptions{"-fPIC"} + end + files { + "Dynamics/*.cpp", + "Dynamics/*.h", + "ConstraintSolver/*.cpp", + "ConstraintSolver/*.h", + "Featherstone/*.cpp", + "Featherstone/*.h", + "MLCPSolvers/*.cpp", + "MLCPSolvers/*.h", + "Vehicle/*.cpp", + "Vehicle/*.h", + "Character/*.cpp", + "Character/*.h" + + } + diff --git a/extern/bullet/src/BulletInverseDynamics/CMakeLists.txt b/extern/bullet/src/BulletInverseDynamics/CMakeLists.txt new file mode 100644 index 000000000000..3331c27eac0e --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/CMakeLists.txt @@ -0,0 +1,66 @@ +INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src ) + +SET(BulletInverseDynamics_SRCS + IDMath.cpp + MultiBodyTree.cpp + details/MultiBodyTreeInitCache.cpp + details/MultiBodyTreeImpl.cpp +) + +SET(BulletInverseDynamicsRoot_HDRS + IDConfig.hpp + IDConfigEigen.hpp + IDMath.hpp + IDConfigBuiltin.hpp + IDErrorMessages.hpp + MultiBodyTree.hpp +) +SET(BulletInverseDynamicsDetails_HDRS + details/IDEigenInterface.hpp + details/IDMatVec.hpp + details/IDLinearMathInterface.hpp + details/MultiBodyTreeImpl.hpp + details/MultiBodyTreeInitCache.hpp +) + +SET(BulletInverseDynamics_HDRS + ${BulletInverseDynamicsRoot_HDRS} + ${BulletInverseDynamicsDetails_HDRS} +) + + +ADD_LIBRARY(BulletInverseDynamics ${BulletInverseDynamics_SRCS} ${BulletInverseDynamics_HDRS}) +SET_TARGET_PROPERTIES(BulletInverseDynamics PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(BulletInverseDynamics PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + TARGET_LINK_LIBRARIES(BulletInverseDynamics Bullet3Common LinearMath) +ENDIF (BUILD_SHARED_LIBS) + + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + #INSTALL of other files requires CMake 2.6 + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletInverseDynamics DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletInverseDynamics RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + INSTALL(FILES ../btBulletCollisionCommon.h +DESTINATION ${INCLUDE_INSTALL_DIR}/BulletInverseDynamics) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(BulletInverseDynamics PROPERTIES FRAMEWORK true) + + SET_TARGET_PROPERTIES(BulletInverseDynamics PROPERTIES PUBLIC_HEADER "${BulletInverseDynamicsRoot_HDRS}") + # Have to list out sub-directories manually: + SET_PROPERTY(SOURCE ${BulletInverseDynamicsDetails_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/details) + + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/extern/bullet/src/BulletInverseDynamics/IDConfig.hpp b/extern/bullet/src/BulletInverseDynamics/IDConfig.hpp new file mode 100644 index 000000000000..ebb10e7a16ef --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/IDConfig.hpp @@ -0,0 +1,107 @@ +///@file Configuration for Inverse Dynamics Library, +/// such as choice of linear algebra library and underlying scalar type +#ifndef IDCONFIG_HPP_ +#define IDCONFIG_HPP_ + +// If true, enable jacobian calculations. +// This adds a 3xN matrix to every body, + 2 3-Vectors. +// so it is not advised for large systems if it is not absolutely necessary. +// Also, this is not required for standard inverse dynamics calculations. +// Will only work with vector math libraries that support 3xN matrices. +#define BT_ID_WITH_JACOBIANS + +// If we have a custom configuration, compile without using other parts of bullet. +#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H +#include +#define BT_ID_WO_BULLET +#define BT_ID_SQRT(x) std::sqrt(x) +#define BT_ID_FABS(x) std::fabs(x) +#define BT_ID_COS(x) std::cos(x) +#define BT_ID_SIN(x) std::sin(x) +#define BT_ID_ATAN2(x, y) std::atan2(x, y) +#define BT_ID_POW(x, y) std::pow(x, y) +#define BT_ID_SNPRINTF snprintf +#define BT_ID_PI M_PI +#define BT_ID_USE_DOUBLE_PRECISION +#else +#define BT_ID_SQRT(x) btSqrt(x) +#define BT_ID_FABS(x) btFabs(x) +#define BT_ID_COS(x) btCos(x) +#define BT_ID_SIN(x) btSin(x) +#define BT_ID_ATAN2(x, y) btAtan2(x, y) +#define BT_ID_POW(x, y) btPow(x, y) +#define BT_ID_PI SIMD_PI +#ifdef _WIN32 + #define BT_ID_SNPRINTF _snprintf +#else + #define BT_ID_SNPRINTF snprintf +#endif // +#endif +// error messages +#include "IDErrorMessages.hpp" + +#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H +/* +#include "IDConfigEigen.hpp" +#include "IDConfigBuiltin.hpp" +*/ +#define INVDYN_INCLUDE_HELPER_2(x) #x +#define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x) +#include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H) +#ifndef btInverseDynamics +#error "custom inverse dynamics config, but no custom namespace defined" +#endif + +#define BT_ID_MAX(a,b) std::max(a,b) +#define BT_ID_MIN(a,b) std::min(a,b) + +#else +#define btInverseDynamics btInverseDynamicsBullet3 +// Use default configuration with bullet's types +// Use the same scalar type as rest of bullet library +#include "LinearMath/btScalar.h" +typedef btScalar idScalar; +#include "LinearMath/btMinMax.h" +#define BT_ID_MAX(a,b) btMax(a,b) +#define BT_ID_MIN(a,b) btMin(a,b) + +#ifdef BT_USE_DOUBLE_PRECISION +#define BT_ID_USE_DOUBLE_PRECISION +#endif + +#ifndef BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 + + +// use bullet types for arrays and array indices +#include "Bullet3Common/b3AlignedObjectArray.h" +// this is to make it work with C++2003, otherwise we could do this: +// template +// using idArray = b3AlignedObjectArray; +template +struct idArray { + typedef b3AlignedObjectArray type; +}; +typedef int idArrayIdx; +#define ID_DECLARE_ALIGNED_ALLOCATOR() B3_DECLARE_ALIGNED_ALLOCATOR() + +#else // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 + +#include "LinearMath/btAlignedObjectArray.h" +template +struct idArray { + typedef btAlignedObjectArray type; +}; +typedef int idArrayIdx; +#define ID_DECLARE_ALIGNED_ALLOCATOR() BT_DECLARE_ALIGNED_ALLOCATOR() + +#endif // BT_USE_INVERSE_DYNAMICS_WITH_BULLET2 + + +// use bullet's allocator functions +#define idMalloc btAllocFunc +#define idFree btFreeFunc + +#define ID_LINEAR_MATH_USE_BULLET +#include "details/IDLinearMathInterface.hpp" +#endif +#endif diff --git a/extern/bullet/src/BulletInverseDynamics/IDConfigBuiltin.hpp b/extern/bullet/src/BulletInverseDynamics/IDConfigBuiltin.hpp new file mode 100644 index 000000000000..130c19c6d6ff --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/IDConfigBuiltin.hpp @@ -0,0 +1,37 @@ +///@file Configuration for Inverse Dynamics Library without external dependencies +#ifndef INVDYNCONFIG_BUILTIN_HPP_ +#define INVDYNCONFIG_BUILTIN_HPP_ +#define btInverseDynamics btInverseDynamicsBuiltin +#ifdef BT_USE_DOUBLE_PRECISION +// choose double/single precision version +typedef double idScalar; +#else +typedef float idScalar; +#endif +// use std::vector for arrays +#include +// this is to make it work with C++2003, otherwise we could do this +// template +// using idArray = std::vector; +template +struct idArray { + typedef std::vector type; +}; +typedef std::vector::size_type idArrayIdx; +// default to standard malloc/free +#include +#define idMalloc ::malloc +#define idFree ::free +// currently not aligned at all... +#define ID_DECLARE_ALIGNED_ALLOCATOR() \ + inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ + inline void operator delete(void* ptr) { idFree(ptr); } \ + inline void* operator new(std::size_t, void* ptr) { return ptr; } \ + inline void operator delete(void*, void*) {} \ + inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ + inline void operator delete[](void* ptr) { idFree(ptr); } \ + inline void* operator new[](std::size_t, void* ptr) { return ptr; } \ + inline void operator delete[](void*, void*) {} + +#include "details/IDMatVec.hpp" +#endif diff --git a/extern/bullet/src/BulletInverseDynamics/IDConfigEigen.hpp b/extern/bullet/src/BulletInverseDynamics/IDConfigEigen.hpp new file mode 100644 index 000000000000..cbd7e8a9c4e2 --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/IDConfigEigen.hpp @@ -0,0 +1,31 @@ +///@file Configuration for Inverse Dynamics Library with Eigen +#ifndef INVDYNCONFIG_EIGEN_HPP_ +#define INVDYNCONFIG_EIGEN_HPP_ +#define btInverseDynamics btInverseDynamicsEigen +#ifdef BT_USE_DOUBLE_PRECISION +// choose double/single precision version +typedef double idScalar; +#else +typedef float idScalar; +#endif + +// use std::vector for arrays +#include +// this is to make it work with C++2003, otherwise we could do this +// template +// using idArray = std::vector; +template +struct idArray { + typedef std::vector type; +}; +typedef std::vector::size_type idArrayIdx; +// default to standard malloc/free +#include +#define ID_DECLARE_ALIGNED_ALLOCATOR() EIGEN_MAKE_ALIGNED_OPERATOR_NEW +// Note on interfaces: +// Eigen::Matrix has data(), to get c-array storage +// HOWEVER: default storage is column-major! +#define ID_LINEAR_MATH_USE_EIGEN +#include "Eigen/Eigen" +#include "details/IDEigenInterface.hpp" +#endif diff --git a/extern/bullet/src/BulletInverseDynamics/IDErrorMessages.hpp b/extern/bullet/src/BulletInverseDynamics/IDErrorMessages.hpp new file mode 100644 index 000000000000..1dc22f860ab6 --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/IDErrorMessages.hpp @@ -0,0 +1,29 @@ +///@file error message utility functions +#ifndef IDUTILS_HPP_ +#define IDUTILS_HPP_ +#include +/// name of file being compiled, without leading path components +#define __INVDYN_FILE_WO_DIR__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__) + +#if !defined(BT_ID_WO_BULLET) && !defined(BT_USE_INVERSE_DYNAMICS_WITH_BULLET2) +#include "Bullet3Common/b3Logging.h" +#define error_message(...) b3Error(__VA_ARGS__) +#define warning_message(...) b3Warning(__VA_ARGS__) +#define id_printf(...) b3Printf(__VA_ARGS__) +#else // BT_ID_WO_BULLET +#include +/// print error message with file/line information +#define error_message(...) \ + do { \ + fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) +/// print warning message with file/line information +#define warning_message(...) \ + do { \ + fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) +#define id_printf(...) printf(__VA_ARGS__) +#endif // BT_ID_WO_BULLET +#endif diff --git a/extern/bullet/src/BulletInverseDynamics/IDMath.cpp b/extern/bullet/src/BulletInverseDynamics/IDMath.cpp new file mode 100644 index 000000000000..99fe20e49287 --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/IDMath.cpp @@ -0,0 +1,437 @@ +#include "IDMath.hpp" + +#include +#include + +namespace btInverseDynamics { +static const idScalar kIsZero = 5 * std::numeric_limits::epsilon(); +// requirements for axis length deviation from 1.0 +// experimentally set from random euler angle rotation matrices +static const idScalar kAxisLengthEpsilon = 10 * kIsZero; + +void setZero(vec3 &v) { + v(0) = 0; + v(1) = 0; + v(2) = 0; +} + +void setZero(vecx &v) { + for (int i = 0; i < v.size(); i++) { + v(i) = 0; + } +} + +void setZero(mat33 &m) { + m(0, 0) = 0; + m(0, 1) = 0; + m(0, 2) = 0; + m(1, 0) = 0; + m(1, 1) = 0; + m(1, 2) = 0; + m(2, 0) = 0; + m(2, 1) = 0; + m(2, 2) = 0; +} + +void skew(vec3& v, mat33* result) { + (*result)(0, 0) = 0.0; + (*result)(0, 1) = -v(2); + (*result)(0, 2) = v(1); + (*result)(1, 0) = v(2); + (*result)(1, 1) = 0.0; + (*result)(1, 2) = -v(0); + (*result)(2, 0) = -v(1); + (*result)(2, 1) = v(0); + (*result)(2, 2) = 0.0; +} + +idScalar maxAbs(const vecx &v) { + idScalar result = 0.0; + for (int i = 0; i < v.size(); i++) { + const idScalar tmp = BT_ID_FABS(v(i)); + if (tmp > result) { + result = tmp; + } + } + return result; +} + +idScalar maxAbs(const vec3 &v) { + idScalar result = 0.0; + for (int i = 0; i < 3; i++) { + const idScalar tmp = BT_ID_FABS(v(i)); + if (tmp > result) { + result = tmp; + } + } + return result; +} + +#if (defined BT_ID_HAVE_MAT3X) +idScalar maxAbsMat3x(const mat3x &m) { + // only used for tests -- so just loop here for portability + idScalar result = 0.0; + for (idArrayIdx col = 0; col < m.cols(); col++) { + for (idArrayIdx row = 0; row < 3; row++) { + result = BT_ID_MAX(result, std::fabs(m(row, col))); + } + } + return result; +} + +void mul(const mat33 &a, const mat3x &b, mat3x *result) { + if (b.cols() != result->cols()) { + error_message("size missmatch. b.cols()= %d, result->cols()= %d\n", + static_cast(b.cols()), static_cast(result->cols())); + abort(); + } + + for (idArrayIdx col = 0; col < b.cols(); col++) { + const idScalar x = a(0,0)*b(0,col)+a(0,1)*b(1,col)+a(0,2)*b(2,col); + const idScalar y = a(1,0)*b(0,col)+a(1,1)*b(1,col)+a(1,2)*b(2,col); + const idScalar z = a(2,0)*b(0,col)+a(2,1)*b(1,col)+a(2,2)*b(2,col); + setMat3xElem(0, col, x, result); + setMat3xElem(1, col, y, result); + setMat3xElem(2, col, z, result); + } +} +void add(const mat3x &a, const mat3x &b, mat3x *result) { + if (a.cols() != b.cols()) { + error_message("size missmatch. a.cols()= %d, b.cols()= %d\n", + static_cast(a.cols()), static_cast(b.cols())); + abort(); + } + for (idArrayIdx col = 0; col < b.cols(); col++) { + for (idArrayIdx row = 0; row < 3; row++) { + setMat3xElem(row, col, a(row, col) + b(row, col), result); + } + } +} +void sub(const mat3x &a, const mat3x &b, mat3x *result) { + if (a.cols() != b.cols()) { + error_message("size missmatch. a.cols()= %d, b.cols()= %d\n", + static_cast(a.cols()), static_cast(b.cols())); + abort(); + } + for (idArrayIdx col = 0; col < b.cols(); col++) { + for (idArrayIdx row = 0; row < 3; row++) { + setMat3xElem(row, col, a(row, col) - b(row, col), result); + } + } +} +#endif + +mat33 transformX(const idScalar &alpha) { + mat33 T; + const idScalar cos_alpha = BT_ID_COS(alpha); + const idScalar sin_alpha = BT_ID_SIN(alpha); + // [1 0 0] + // [0 c s] + // [0 -s c] + T(0, 0) = 1.0; + T(0, 1) = 0.0; + T(0, 2) = 0.0; + + T(1, 0) = 0.0; + T(1, 1) = cos_alpha; + T(1, 2) = sin_alpha; + + T(2, 0) = 0.0; + T(2, 1) = -sin_alpha; + T(2, 2) = cos_alpha; + + return T; +} + +mat33 transformY(const idScalar &beta) { + mat33 T; + const idScalar cos_beta = BT_ID_COS(beta); + const idScalar sin_beta = BT_ID_SIN(beta); + // [c 0 -s] + // [0 1 0] + // [s 0 c] + T(0, 0) = cos_beta; + T(0, 1) = 0.0; + T(0, 2) = -sin_beta; + + T(1, 0) = 0.0; + T(1, 1) = 1.0; + T(1, 2) = 0.0; + + T(2, 0) = sin_beta; + T(2, 1) = 0.0; + T(2, 2) = cos_beta; + + return T; +} + +mat33 transformZ(const idScalar &gamma) { + mat33 T; + const idScalar cos_gamma = BT_ID_COS(gamma); + const idScalar sin_gamma = BT_ID_SIN(gamma); + // [ c s 0] + // [-s c 0] + // [ 0 0 1] + T(0, 0) = cos_gamma; + T(0, 1) = sin_gamma; + T(0, 2) = 0.0; + + T(1, 0) = -sin_gamma; + T(1, 1) = cos_gamma; + T(1, 2) = 0.0; + + T(2, 0) = 0.0; + T(2, 1) = 0.0; + T(2, 2) = 1.0; + + return T; +} + +mat33 tildeOperator(const vec3 &v) { + mat33 m; + m(0, 0) = 0.0; + m(0, 1) = -v(2); + m(0, 2) = v(1); + m(1, 0) = v(2); + m(1, 1) = 0.0; + m(1, 2) = -v(0); + m(2, 0) = -v(1); + m(2, 1) = v(0); + m(2, 2) = 0.0; + return m; +} + +void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) { + const idScalar sa = BT_ID_SIN(alpha); + const idScalar ca = BT_ID_COS(alpha); + const idScalar st = BT_ID_SIN(theta); + const idScalar ct = BT_ID_COS(theta); + + (*r)(0) = a; + (*r)(1) = -sa * d; + (*r)(2) = ca * d; + + (*T)(0, 0) = ct; + (*T)(0, 1) = -st; + (*T)(0, 2) = 0.0; + + (*T)(1, 0) = st * ca; + (*T)(1, 1) = ct * ca; + (*T)(1, 2) = -sa; + + (*T)(2, 0) = st * sa; + (*T)(2, 1) = ct * sa; + (*T)(2, 2) = ca; +} + +void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) { + const idScalar c = BT_ID_COS(angle); + const idScalar s = -BT_ID_SIN(angle); + const idScalar one_m_c = 1.0 - c; + + const idScalar &x = axis(0); + const idScalar &y = axis(1); + const idScalar &z = axis(2); + + (*T)(0, 0) = x * x * one_m_c + c; + (*T)(0, 1) = x * y * one_m_c - z * s; + (*T)(0, 2) = x * z * one_m_c + y * s; + + (*T)(1, 0) = x * y * one_m_c + z * s; + (*T)(1, 1) = y * y * one_m_c + c; + (*T)(1, 2) = y * z * one_m_c - x * s; + + (*T)(2, 0) = x * z * one_m_c - y * s; + (*T)(2, 1) = y * z * one_m_c + x * s; + (*T)(2, 2) = z * z * one_m_c + c; +} + +bool isPositiveDefinite(const mat33 &m) { + // test if all upper left determinants are positive + if (m(0, 0) <= 0) { // upper 1x1 + return false; + } + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0) { // upper 2x2 + return false; + } + if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - + m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) { + return false; + } + return true; +} + +bool isPositiveSemiDefinite(const mat33 &m) { + // test if all upper left determinants are positive + if (m(0, 0) < 0) { // upper 1x1 + return false; + } + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0) { // upper 2x2 + return false; + } + if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - + m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) { + return false; + } + return true; +} + +bool isPositiveSemiDefiniteFuzzy(const mat33 &m) { + // test if all upper left determinants are positive + if (m(0, 0) < -kIsZero) { // upper 1x1 + return false; + } + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero) { // upper 2x2 + return false; + } + if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - + m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero) { + return false; + } + return true; +} + +idScalar determinant(const mat33 &m) { + return m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) - + m(0, 2) * m(1, 1) * m(2, 0) - m(0, 0) * m(1, 2) * m(2, 1) - m(0, 1) * m(1, 0) * m(2, 2); +} + +bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint) { + // TODO(Thomas) do we really want this? + // in cases where the inertia tensor about the center of mass is zero, + // the determinant of the inertia tensor about the joint axis is almost + // zero and can have a very small negative value. + if (!isPositiveSemiDefiniteFuzzy(I)) { + error_message("invalid inertia matrix for body %d, not positive definite " + "(fixed joint)\n", + index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + + return false; + } + + // check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k) + if (!has_fixed_joint) { + if (I(0, 0) + I(1, 1) < I(2, 2)) { + error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + if (I(0, 0) + I(1, 1) < I(2, 2)) { + error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + if (I(1, 1) + I(2, 2) < I(0, 0)) { + error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + } + // check positive/zero diagonal elements + for (int i = 0; i < 3; i++) { + if (I(i, i) < 0) { // accept zero + error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i)); + return false; + } + } + // check symmetry + if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero) { + error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= " + "%e\n", + index, I(1, 0) - I(0, 1)); + return false; + } + if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero) { + error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= " + "%e\n", + index, I(2, 0) - I(0, 2)); + return false; + } + if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero) { + error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index, + I(1, 2) - I(2, 1)); + return false; + } + return true; +} + +bool isValidTransformMatrix(const mat33 &m) { +#define print_mat(x) \ + error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \ + x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2)) + + // check for unit length column vectors + for (int i = 0; i < 3; i++) { + const idScalar length_minus_1 = + BT_ID_FABS(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0); + if (length_minus_1 > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (column %d not unit length)\n" + "column = [%.18e %.18e %.18e]\n" + "length-1.0= %.18e\n", + i, m(0, i), m(1, i), m(2, i), length_minus_1); + print_mat(m); + return false; + } + } + // check for orthogonal column vectors + if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n"); + print_mat(m); + return false; + } + if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n"); + print_mat(m); + return false; + } + if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n"); + print_mat(m); + return false; + } + // check determinant (rotation not reflection) + if (determinant(m) <= 0) { + error_message("Not a valid rotation matrix (determinant <=0)\n"); + print_mat(m); + return false; + } + return true; +} + +bool isUnitVector(const vec3 &vector) { + return BT_ID_FABS(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) < + kIsZero; +} + +vec3 rpyFromMatrix(const mat33 &rot) { + vec3 rpy; + rpy(2) = BT_ID_ATAN2(-rot(1, 0), rot(0, 0)); + rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0)); + rpy(0) = BT_ID_ATAN2(-rot(2, 0), rot(2, 2)); + return rpy; +} +} diff --git a/extern/bullet/src/BulletInverseDynamics/IDMath.hpp b/extern/bullet/src/BulletInverseDynamics/IDMath.hpp new file mode 100644 index 000000000000..b355474d44e2 --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/IDMath.hpp @@ -0,0 +1,99 @@ +/// @file Math utility functions used in inverse dynamics library. +/// Defined here as they may not be provided by the math library. + +#ifndef IDMATH_HPP_ +#define IDMATH_HPP_ +#include "IDConfig.hpp" + +namespace btInverseDynamics { +/// set all elements to zero +void setZero(vec3& v); +/// set all elements to zero +void setZero(vecx& v); +/// set all elements to zero +void setZero(mat33& m); +/// create a skew symmetric matrix from a vector (useful for cross product abstraction, e.g. v x a = V * a) +void skew(vec3& v, mat33* result); +/// return maximum absolute value +idScalar maxAbs(const vecx& v); +#ifndef ID_LINEAR_MATH_USE_EIGEN +/// return maximum absolute value +idScalar maxAbs(const vec3& v); +#endif //ID_LINEAR_MATH_USE_EIGEN + +#if (defined BT_ID_HAVE_MAT3X) +idScalar maxAbsMat3x(const mat3x& m); +void setZero(mat3x&m); +// define math functions on mat3x here to avoid allocations in operators. +void mul(const mat33&a, const mat3x&b, mat3x* result); +void add(const mat3x&a, const mat3x&b, mat3x* result); +void sub(const mat3x&a, const mat3x&b, mat3x* result); +#endif + +/// get offset vector & transform matrix from DH parameters +/// TODO: add documentation +void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3* r, mat33* T); + +/// Check if a 3x3 matrix is positive definite +/// @param m a 3x3 matrix +/// @return true if m>0, false otherwise +bool isPositiveDefinite(const mat33& m); + +/// Check if a 3x3 matrix is positive semi definite +/// @param m a 3x3 matrix +/// @return true if m>=0, false otherwise +bool isPositiveSemiDefinite(const mat33& m); +/// Check if a 3x3 matrix is positive semi definite within numeric limits +/// @param m a 3x3 matrix +/// @return true if m>=-eps, false otherwise +bool isPositiveSemiDefiniteFuzzy(const mat33& m); + +/// Determinant of 3x3 matrix +/// NOTE: implemented here for portability, as determinant operation +/// will be implemented differently for various matrix/vector libraries +/// @param m a 3x3 matrix +/// @return det(m) +idScalar determinant(const mat33& m); + +/// Test if a 3x3 matrix satisfies some properties of inertia matrices +/// @param I a 3x3 matrix +/// @param index body index (for error messages) +/// @param has_fixed_joint: if true, positive semi-definite matrices are accepted +/// @return true if I satisfies inertia matrix properties, false otherwise. +bool isValidInertiaMatrix(const mat33& I, int index, bool has_fixed_joint); + +/// Check if a 3x3 matrix is a valid transform (rotation) matrix +/// @param m a 3x3 matrix +/// @return true if m is a rotation matrix, false otherwise +bool isValidTransformMatrix(const mat33& m); +/// Transform matrix from parent to child frame, +/// when the child frame is rotated about @param axis by @angle +/// (mathematically positive) +/// @param axis the axis of rotation +/// @param angle rotation angle +/// @param T pointer to transform matrix +void bodyTParentFromAxisAngle(const vec3& axis, const idScalar& angle, mat33* T); + +/// Check if this is a unit vector +/// @param vector +/// @return true if |vector|=1 within numeric limits +bool isUnitVector(const vec3& vector); + +/// @input a vector in R^3 +/// @returns corresponding spin tensor +mat33 tildeOperator(const vec3& v); +/// @param alpha angle in radians +/// @returns transform matrix for ratation with @param alpha about x-axis +mat33 transformX(const idScalar& alpha); +/// @param beta angle in radians +/// @returns transform matrix for ratation with @param beta about y-axis +mat33 transformY(const idScalar& beta); +/// @param gamma angle in radians +/// @returns transform matrix for ratation with @param gamma about z-axis +mat33 transformZ(const idScalar& gamma); +///calculate rpy angles (x-y-z Euler angles) from a given rotation matrix +/// @param rot rotation matrix +/// @returns x-y-z Euler angles +vec3 rpyFromMatrix(const mat33&rot); +} +#endif // IDMATH_HPP_ diff --git a/extern/bullet/src/BulletInverseDynamics/MultiBodyTree.cpp b/extern/bullet/src/BulletInverseDynamics/MultiBodyTree.cpp new file mode 100644 index 000000000000..0f6668fdbbd2 --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/MultiBodyTree.cpp @@ -0,0 +1,461 @@ +#include "MultiBodyTree.hpp" + +#include +#include +#include + +#include "IDMath.hpp" +#include "details/MultiBodyTreeImpl.hpp" +#include "details/MultiBodyTreeInitCache.hpp" + +namespace btInverseDynamics { + +MultiBodyTree::MultiBodyTree() + : m_is_finalized(false), + m_mass_parameters_are_valid(true), + m_accept_invalid_mass_parameters(false), + m_impl(0x0), + m_init_cache(0x0) { + m_init_cache = new InitCache(); +} + +MultiBodyTree::~MultiBodyTree() { + delete m_impl; + delete m_init_cache; +} + +void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) { + m_accept_invalid_mass_parameters = flag; +} + +bool MultiBodyTree::getAcceptInvalidMassProperties() const { + return m_accept_invalid_mass_parameters; +} + +int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const { + return m_impl->getBodyOrigin(body_index, world_origin); +} + +int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const { + return m_impl->getBodyCoM(body_index, world_com); +} + +int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const { + return m_impl->getBodyTransform(body_index, world_T_body); +} +int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const { + return m_impl->getBodyAngularVelocity(body_index, world_omega); +} +int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const { + return m_impl->getBodyLinearVelocity(body_index, world_velocity); +} + +int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const { + return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity); +} + +int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const { + return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega); +} +int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const { + return m_impl->getBodyLinearAcceleration(body_index, world_acceleration); +} + +int MultiBodyTree::getParentRParentBodyRef(const int body_index, vec3* r) const { + return m_impl->getParentRParentBodyRef(body_index, r); +} + +int MultiBodyTree::getBodyTParentRef(const int body_index, mat33* T) const { + return m_impl->getBodyTParentRef(body_index, T); +} + +int MultiBodyTree::getBodyAxisOfMotion(const int body_index, vec3* axis) const { + return m_impl->getBodyAxisOfMotion(body_index, axis); +} + +void MultiBodyTree::printTree() { m_impl->printTree(); } +void MultiBodyTree::printTreeData() { m_impl->printTreeData(); } + +int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; } + +int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; } + +int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u, + vecx *joint_forces) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) { + error_message("error in inverse dynamics calculation\n"); + return -1; + } + return 0; +} + +int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics, + const bool initialize_matrix, + const bool set_lower_triangular_matrix, matxx *mass_matrix) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == + m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix, + set_lower_triangular_matrix, mass_matrix)) { + error_message("error in mass matrix calculation\n"); + return -1; + } + return 0; +} + +int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) { + return calculateMassMatrix(q, true, true, true, mass_matrix); +} + + + +int MultiBodyTree::calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u) { + vec3 world_gravity(m_impl->m_world_gravity); + // temporarily set gravity to zero, to ensure we get the actual accelerations + setZero(m_impl->m_world_gravity); + + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateKinematics(q, u, dot_u, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY_ACCELERATION)) { + error_message("error in kinematics calculation\n"); + return -1; + } + + m_impl->m_world_gravity=world_gravity; + return 0; +} + + +int MultiBodyTree::calculatePositionKinematics(const vecx& q) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateKinematics(q, q, q, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { + error_message("error in kinematics calculation\n"); + return -1; + } + return 0; +} + +int MultiBodyTree::calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateKinematics(q, u, u, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { + error_message("error in kinematics calculation\n"); + return -1; + } + return 0; +} + + +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) +int MultiBodyTree::calculateJacobians(const vecx& q, const vecx& u) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateJacobians(q, u, + MultiBodyTree::MultiBodyImpl::POSITION_VELOCITY)) { + error_message("error in jacobian calculation\n"); + return -1; + } + return 0; +} + +int MultiBodyTree::calculateJacobians(const vecx& q){ + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateJacobians(q, q, + MultiBodyTree::MultiBodyImpl::POSITION_ONLY)) { + error_message("error in jacobian calculation\n"); + return -1; + } + return 0; +} + +int MultiBodyTree::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const { + return m_impl->getBodyDotJacobianTransU(body_index,world_dot_jac_trans_u); +} + +int MultiBodyTree::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const { + return m_impl->getBodyDotJacobianRotU(body_index,world_dot_jac_rot_u); +} + +int MultiBodyTree::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const { + return m_impl->getBodyJacobianTrans(body_index,world_jac_trans); +} + +int MultiBodyTree::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const { + return m_impl->getBodyJacobianRot(body_index,world_jac_rot); +} + + +#endif + +int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type, + const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref, + const vec3 &body_axis_of_motion_, idScalar mass, + const vec3 &body_r_body_com, const mat33 &body_I_body, + const int user_int, void *user_ptr) { + if (body_index < 0) { + error_message("body index must be positive (got %d)\n", body_index); + return -1; + } + vec3 body_axis_of_motion(body_axis_of_motion_); + switch (joint_type) { + case REVOLUTE: + case PRISMATIC: + // check if axis is unit vector + if (!isUnitVector(body_axis_of_motion)) { + warning_message( + "axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n", + body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2)); + idScalar length = BT_ID_SQRT(BT_ID_POW(body_axis_of_motion(0), 2) + + BT_ID_POW(body_axis_of_motion(1), 2) + + BT_ID_POW(body_axis_of_motion(2), 2)); + if (length < BT_ID_SQRT(std::numeric_limits::min())) { + error_message("axis of motion vector too short (%e)\n", length); + return -1; + } + body_axis_of_motion = (1.0 / length) * body_axis_of_motion; + } + break; + case FIXED: + break; + case FLOATING: + break; + default: + error_message("unknown joint type %d\n", joint_type); + return -1; + } + + // sanity check for mass properties. Zero mass is OK. + if (mass < 0) { + m_mass_parameters_are_valid = false; + error_message("Body %d has invalid mass %e\n", body_index, mass); + if (!m_accept_invalid_mass_parameters) { + return -1; + } + } + + if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) { + m_mass_parameters_are_valid = false; + // error message printed in function call + if (!m_accept_invalid_mass_parameters) { + return -1; + } + } + + if (!isValidTransformMatrix(body_T_parent_ref)) { + return -1; + } + + return m_init_cache->addBody(body_index, parent_index, joint_type, parent_r_parent_body_ref, + body_T_parent_ref, body_axis_of_motion, mass, body_r_body_com, + body_I_body, user_int, user_ptr); +} + +int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const { + return m_impl->getParentIndex(body_index, parent_index); +} + +int MultiBodyTree::getUserInt(const int body_index, int *user_int) const { + return m_impl->getUserInt(body_index, user_int); +} + +int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const { + return m_impl->getUserPtr(body_index, user_ptr); +} + +int MultiBodyTree::setUserInt(const int body_index, const int user_int) { + return m_impl->setUserInt(body_index, user_int); +} + +int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) { + return m_impl->setUserPtr(body_index, user_ptr); +} + +int MultiBodyTree::finalize() { + const int &num_bodies = m_init_cache->numBodies(); + const int &num_dofs = m_init_cache->numDoFs(); + + if(num_dofs<=0) { + error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs); + //return -1; + } + + // 1 allocate internal MultiBody structure + m_impl = new MultiBodyImpl(num_bodies, num_dofs); + + // 2 build new index set assuring index(parent) < index(child) + if (-1 == m_init_cache->buildIndexSets()) { + return -1; + } + m_init_cache->getParentIndexArray(&m_impl->m_parent_index); + + // 3 setup internal kinematic and dynamic data + for (int index = 0; index < num_bodies; index++) { + InertiaData inertia; + JointData joint; + if (-1 == m_init_cache->getInertiaData(index, &inertia)) { + return -1; + } + if (-1 == m_init_cache->getJointData(index, &joint)) { + return -1; + } + + RigidBody &rigid_body = m_impl->m_body_list[index]; + + rigid_body.m_mass = inertia.m_mass; + rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com; + rigid_body.m_body_I_body = inertia.m_body_I_body; + rigid_body.m_joint_type = joint.m_type; + rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; + rigid_body.m_body_T_parent_ref = joint.m_child_T_parent_ref; + rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; + rigid_body.m_joint_type = joint.m_type; + + int user_int; + if (-1 == m_init_cache->getUserInt(index, &user_int)) { + return -1; + } + if (-1 == m_impl->setUserInt(index, user_int)) { + return -1; + } + + void* user_ptr; + if (-1 == m_init_cache->getUserPtr(index, &user_ptr)) { + return -1; + } + if (-1 == m_impl->setUserPtr(index, user_ptr)) { + return -1; + } + + // Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized + // matrices. + switch (rigid_body.m_joint_type) { + case REVOLUTE: + rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0); + rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1); + rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2); + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + case PRISMATIC: + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0); + rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1); + rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2); + break; + case FIXED: + // NOTE/TODO: dimension really should be zero .. + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + case FLOATING: + // NOTE/TODO: this is not really correct. + // the Jacobians should be 3x3 matrices here ! + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + default: + error_message("unsupported joint type %d\n", rigid_body.m_joint_type); + return -1; + } + } + + // 4 assign degree of freedom indices & build per-joint-type index arrays + if (-1 == m_impl->generateIndexSets()) { + error_message("generating index sets\n"); + return -1; + } + + // 5 do some pre-computations .. + m_impl->calculateStaticData(); + + // 6. make sure all user forces are set to zero, as this might not happen + // in the vector ctors. + m_impl->clearAllUserForcesAndMoments(); + + m_is_finalized = true; + return 0; +} + +int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) { + return m_impl->setGravityInWorldFrame(gravity); +} + +int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const { + return m_impl->getJointType(body_index, joint_type); +} + +int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const { + return m_impl->getJointTypeStr(body_index, joint_type); +} + +int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const { + return m_impl->getDoFOffset(body_index, q_offset); +} + +int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) { + return m_impl->setBodyMass(body_index, mass); +} + +int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment) { + return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment); +} + +int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment) { + return m_impl->setBodySecondMassMoment(body_index, second_mass_moment); +} + +int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const { + return m_impl->getBodyMass(body_index, mass); +} + +int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const { + return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment); +} + +int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const { + return m_impl->getBodySecondMassMoment(body_index, second_mass_moment); +} + +void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); } + +int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force) { + return m_impl->addUserForce(body_index, body_force); +} + +int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) { + return m_impl->addUserMoment(body_index, body_moment); +} + +} diff --git a/extern/bullet/src/BulletInverseDynamics/MultiBodyTree.hpp b/extern/bullet/src/BulletInverseDynamics/MultiBodyTree.hpp new file mode 100644 index 000000000000..d235aa6e7613 --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/MultiBodyTree.hpp @@ -0,0 +1,363 @@ +#ifndef MULTIBODYTREE_HPP_ +#define MULTIBODYTREE_HPP_ + +#include "IDConfig.hpp" +#include "IDMath.hpp" + +namespace btInverseDynamics { + +/// Enumeration of supported joint types +enum JointType { + /// no degree of freedom, moves with parent + FIXED = 0, + /// one rotational degree of freedom relative to parent + REVOLUTE, + /// one translational degree of freedom relative to parent + PRISMATIC, + /// six degrees of freedom relative to parent + FLOATING +}; + +/// Interface class for calculating inverse dynamics for tree structured +/// multibody systems +/// +/// Note on degrees of freedom +/// The q vector contains the generalized coordinate set defining the tree's configuration. +/// Every joint adds elements that define the corresponding link's frame pose relative to +/// its parent. For the joint types that is: +/// - FIXED: none +/// - REVOLUTE: angle of rotation [rad] +/// - PRISMATIC: displacement [m] +/// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m] +/// (in that order) +/// The u vector contains the generalized speeds, which are +/// - FIXED: none +/// - REVOLUTE: time derivative of angle of rotation [rad/s] +/// - PRISMATIC: time derivative of displacement [m/s] +/// - FLOATING: angular velocity [rad/s] (*not* time derivative of rpy angles) +/// and time derivative of displacement in parent frame [m/s] +/// +/// The q and u vectors are obtained by stacking contributions of all bodies in one +/// vector in the order of body indices. +/// +/// Note on generalized forces: analogous to u, i.e., +/// - FIXED: none +/// - REVOLUTE: moment [Nm], about joint axis +/// - PRISMATIC: force [N], along joint axis +/// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame +/// (in that order) +/// +/// TODO - force element interface (friction, springs, dampers, etc) +/// - gears and motor inertia +class MultiBodyTree { +public: + ID_DECLARE_ALIGNED_ALLOCATOR(); + /// The contructor. + /// Initialization & allocation is via addBody and buildSystem calls. + MultiBodyTree(); + /// the destructor. This also deallocates all memory + ~MultiBodyTree(); + + /// Add body to the system. this allocates memory and not real-time safe. + /// This only adds the data to an initial cache. After all bodies have been + /// added, + /// the system is setup using the buildSystem call + /// @param body_index index of the body to be added. Must >=0, =dim(u) + /// @param dot_u time derivative of u + /// @param joint_forces this is where the resulting joint forces will be + /// stored. dim(joint_forces) = dim(u) + /// @return 0 on success, -1 on error + int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, + vecx* joint_forces); + /// Calculate joint space mass matrix + /// @param q generalized coordinates + /// @param initialize_matrix if true, initialize mass matrix with zero. + /// If mass_matrix is initialized to zero externally and only used + /// for mass matrix computations for the same system, it is safe to + /// set this to false. + /// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix + /// is also populated, otherwise not. + /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) + /// @return -1 on error, 0 on success + int calculateMassMatrix(const vecx& q, const bool update_kinematics, + const bool initialize_matrix, const bool set_lower_triangular_matrix, + matxx* mass_matrix); + + /// Calculate joint space mass matrix. + /// This version will update kinematics, initialize all mass_matrix elements to zero and + /// populate all mass matrix entries. + /// @param q generalized coordinates + /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) + /// @return -1 on error, 0 on success + int calculateMassMatrix(const vecx& q, matxx* mass_matrix); + + + /// Calculates kinematics also calculated in calculateInverseDynamics, + /// but not dynamics. + /// This function ensures that correct accelerations are computed that do not + /// contain gravitational acceleration terms. + /// Does not calculate Jacobians, but only vector quantities (positions, velocities & accelerations) + int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u); + /// Calculate position kinematics + int calculatePositionKinematics(const vecx& q); + /// Calculate position and velocity kinematics + int calculatePositionAndVelocityKinematics(const vecx& q, const vecx& u); + +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) + /// Calculate Jacobians (dvel/du), as well as velocity-dependent accelearation components + /// d(Jacobian)/dt*u + /// This function assumes that calculateInverseDynamics was called, or calculateKinematics, + /// or calculatePositionAndVelocityKinematics + int calculateJacobians(const vecx& q, const vecx& u); + /// Calculate Jacobians (dvel/du) + /// This function assumes that calculateInverseDynamics was called, or + /// one of the calculateKineamtics functions + int calculateJacobians(const vecx& q); +#endif // BT_ID_HAVE_MAT3X + + + /// set gravitational acceleration + /// the default is [0;0;-9.8] in the world frame + /// @param gravity the gravitational acceleration in world frame + /// @return 0 on success, -1 on error + int setGravityInWorldFrame(const vec3& gravity); + /// returns number of bodies in tree + int numBodies() const; + /// returns number of mechanical degrees of freedom (dimension of q-vector) + int numDoFs() const; + /// get origin of a body-fixed frame, represented in world frame + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyOrigin(const int body_index, vec3* world_origin) const; + /// get center of mass of a body, represented in world frame + /// @param body_index index for frame/body + /// @param world_com pointer for return data + /// @return 0 on success, -1 on error + int getBodyCoM(const int body_index, vec3* world_com) const; + /// get transform from of a body-fixed frame to the world frame + /// @param body_index index for frame/body + /// @param world_T_body pointer for return data + /// @return 0 on success, -1 on error + int getBodyTransform(const int body_index, mat33* world_T_body) const; + /// get absolute angular velocity for a body, represented in the world frame + /// @param body_index index for frame/body + /// @param world_omega pointer for return data + /// @return 0 on success, -1 on error + int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; + /// get linear velocity of a body, represented in world frame + /// @param body_index index for frame/body + /// @param world_velocity pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; + /// get linear velocity of a body's CoM, represented in world frame + /// (not required for inverse dynamics, provided for convenience) + /// @param body_index index for frame/body + /// @param world_vel_com pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; + /// get origin of a body-fixed frame, represented in world frame + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; + /// get origin of a body-fixed frame, represented in world frame + /// NOTE: this will include the gravitational acceleration, so the actual acceleration is + /// obtainened by setting gravitational acceleration to zero, or subtracting it. + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; + +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) + // get translational jacobian, in world frame (dworld_velocity/du) + int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const; + // get rotational jacobian, in world frame (dworld_omega/du) + int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const; + // get product of translational jacobian derivative * generatlized velocities + int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const; + // get product of rotational jacobian derivative * generatlized velocities + int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const; +#endif // BT_ID_HAVE_MAT3X + + /// returns the (internal) index of body + /// @param body_index is the index of a body + /// @param parent_index pointer to where parent index will be stored + /// @return 0 on success, -1 on error + int getParentIndex(const int body_index, int* parent_index) const; + /// get joint type + /// @param body_index index of the body + /// @param joint_type the corresponding joint type + /// @return 0 on success, -1 on failure + int getJointType(const int body_index, JointType* joint_type) const; + /// get joint type as string + /// @param body_index index of the body + /// @param joint_type string naming the corresponding joint type + /// @return 0 on success, -1 on failure + int getJointTypeStr(const int body_index, const char** joint_type) const; + /// get offset translation to parent body (see addBody) + /// @param body_index index of the body + /// @param r the offset translation (see above) + /// @return 0 on success, -1 on failure + int getParentRParentBodyRef(const int body_index, vec3* r) const; + /// get offset rotation to parent body (see addBody) + /// @param body_index index of the body + /// @param T the transform (see above) + /// @return 0 on success, -1 on failure + int getBodyTParentRef(const int body_index, mat33* T) const; + /// get axis of motion (see addBody) + /// @param body_index index of the body + /// @param axis the axis (see above) + /// @return 0 on success, -1 on failure + int getBodyAxisOfMotion(const int body_index, vec3* axis) const; + /// get offset for degrees of freedom of this body into the q-vector + /// @param body_index index of the body + /// @param q_offset offset the q vector + /// @return -1 on error, 0 on success + int getDoFOffset(const int body_index, int* q_offset) const; + /// get user integer. not used by the library. + /// @param body_index index of the body + /// @param user_int the user integer + /// @return 0 on success, -1 on error + int getUserInt(const int body_index, int* user_int) const; + /// get user pointer. not used by the library. + /// @param body_index index of the body + /// @param user_ptr the user pointer + /// @return 0 on success, -1 on error + int getUserPtr(const int body_index, void** user_ptr) const; + /// set user integer. not used by the library. + /// @param body_index index of the body + /// @param user_int the user integer + /// @return 0 on success, -1 on error + int setUserInt(const int body_index, const int user_int); + /// set user pointer. not used by the library. + /// @param body_index index of the body + /// @param user_ptr the user pointer + /// @return 0 on success, -1 on error + int setUserPtr(const int body_index, void* const user_ptr); + /// set mass for a body + /// @param body_index index of the body + /// @param mass the mass to set + /// @return 0 on success, -1 on failure + int setBodyMass(const int body_index, const idScalar mass); + /// set first moment of mass for a body + /// (mass * center of mass, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param first_mass_moment the vector to set + /// @return 0 on success, -1 on failure + int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); + /// set second moment of mass for a body + /// (moment of inertia, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param second_mass_moment the inertia matrix + /// @return 0 on success, -1 on failure + int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); + /// get mass for a body + /// @param body_index index of the body + /// @param mass the mass + /// @return 0 on success, -1 on failure + int getBodyMass(const int body_index, idScalar* mass) const; + /// get first moment of mass for a body + /// (mass * center of mass, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param first_moment the vector + /// @return 0 on success, -1 on failure + int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; + /// get second moment of mass for a body + /// (moment of inertia, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param second_mass_moment the inertia matrix + /// @return 0 on success, -1 on failure + int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; + /// set all user forces and moments to zero + void clearAllUserForcesAndMoments(); + /// Add an external force to a body, acting at the origin of the body-fixed frame. + /// Calls to addUserForce are cumulative. Set the user force and moment to zero + /// via clearAllUserForcesAndMoments() + /// @param body_force the force represented in the body-fixed frame of reference + /// @return 0 on success, -1 on error + int addUserForce(const int body_index, const vec3& body_force); + /// Add an external moment to a body. + /// Calls to addUserMoment are cumulative. Set the user force and moment to zero + /// via clearAllUserForcesAndMoments() + /// @param body_moment the moment represented in the body-fixed frame of reference + /// @return 0 on success, -1 on error + int addUserMoment(const int body_index, const vec3& body_moment); + +private: + // flag indicating if system has been initialized + bool m_is_finalized; + // flag indicating if mass properties are physically valid + bool m_mass_parameters_are_valid; + // flag defining if unphysical mass parameters are accepted + bool m_accept_invalid_mass_parameters; + // This struct implements the inverse dynamics calculations + class MultiBodyImpl; + MultiBodyImpl* m_impl; + // cache data structure for initialization + class InitCache; + InitCache* m_init_cache; +}; +} // namespace btInverseDynamics +#endif // MULTIBODYTREE_HPP_ diff --git a/extern/bullet/src/BulletInverseDynamics/details/IDEigenInterface.hpp b/extern/bullet/src/BulletInverseDynamics/details/IDEigenInterface.hpp new file mode 100644 index 000000000000..836395cea22e --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/details/IDEigenInterface.hpp @@ -0,0 +1,36 @@ +#ifndef INVDYNEIGENINTERFACE_HPP_ +#define INVDYNEIGENINTERFACE_HPP_ +#include "../IDConfig.hpp" +namespace btInverseDynamics { + +#define BT_ID_HAVE_MAT3X + +#ifdef BT_USE_DOUBLE_PRECISION +typedef Eigen::Matrix vecx; +typedef Eigen::Matrix vec3; +typedef Eigen::Matrix mat33; +typedef Eigen::Matrix matxx; +typedef Eigen::Matrix mat3x; +#else +typedef Eigen::Matrix vecx; +typedef Eigen::Matrix vec3; +typedef Eigen::Matrix mat33; +typedef Eigen::Matrix matxx; +typedef Eigen::Matrix mat3x; +#endif + +inline void resize(mat3x &m, Eigen::Index size) { + m.resize(3, size); + m.setZero(); +} + +inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){ + (*m)(row, col) = val; +} + +inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){ + (*m)(row, col) = val; +} + +} +#endif // INVDYNEIGENINTERFACE_HPP_ diff --git a/extern/bullet/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/extern/bullet/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp new file mode 100644 index 000000000000..a44f779bec2d --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -0,0 +1,172 @@ +#ifndef IDLINEARMATHINTERFACE_HPP_ +#define IDLINEARMATHINTERFACE_HPP_ + +#include + +#include "../IDConfig.hpp" + +#include "../../LinearMath/btMatrix3x3.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btMatrixX.h" +#define BT_ID_HAVE_MAT3X + +namespace btInverseDynamics { +class vec3; +class vecx; +class mat33; +typedef btMatrixX matxx; + +class vec3 : public btVector3 { +public: + vec3() : btVector3() {} + vec3(const btVector3& btv) { *this = btv; } + idScalar& operator()(int i) { return (*this)[i]; } + const idScalar& operator()(int i) const { return (*this)[i]; } + int size() const { return 3; } + const vec3& operator=(const btVector3& rhs) { + *static_cast(this) = rhs; + return *this; + } +}; + +class mat33 : public btMatrix3x3 { +public: + mat33() : btMatrix3x3() {} + mat33(const btMatrix3x3& btm) { *this = btm; } + idScalar& operator()(int i, int j) { return (*this)[i][j]; } + const idScalar& operator()(int i, int j) const { return (*this)[i][j]; } + const mat33& operator=(const btMatrix3x3& rhs) { + *static_cast(this) = rhs; + return *this; + } + friend mat33 operator*(const idScalar& s, const mat33& a); + friend mat33 operator/(const mat33& a, const idScalar& s); +}; + +inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s); } + +inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } + +class vecx : public btVectorX { +public: + vecx(int size) : btVectorX(size) {} + const vecx& operator=(const btVectorX& rhs) { + *static_cast*>(this) = rhs; + return *this; + } + + idScalar& operator()(int i) { return (*this)[i]; } + const idScalar& operator()(int i) const { return (*this)[i]; } + + friend vecx operator*(const vecx& a, const idScalar& s); + friend vecx operator*(const idScalar& s, const vecx& a); + + friend vecx operator+(const vecx& a, const vecx& b); + friend vecx operator-(const vecx& a, const vecx& b); + friend vecx operator/(const vecx& a, const idScalar& s); +}; + +inline vecx operator*(const vecx& a, const idScalar& s) { + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result(i) = a(i) * s; + } + return result; +} +inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; } +inline vecx operator+(const vecx& a, const vecx& b) { + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result(i) = a(i) + b(i); + } + + return result; +} + +inline vecx operator-(const vecx& a, const vecx& b) { + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result(i) = a(i) - b(i); + } + return result; +} +inline vecx operator/(const vecx& a, const idScalar& s) { + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result(i) = a(i) / s; + } + + return result; +} + +// use btMatrixX to implement 3xX matrix +class mat3x : public matxx { +public: + mat3x(){} + mat3x(const mat3x&rhs) { + matxx::resize(rhs.rows(), rhs.cols()); + *this = rhs; + } + mat3x(int rows, int cols): matxx(3,cols) { + } + void operator=(const mat3x& rhs) { + if (m_cols != rhs.m_cols) { + error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); + abort(); + } + for(int i=0;isetElem(row, col, val); +} + +inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){ + m->setElem(row, col, val); +} + +} + +#endif // IDLINEARMATHINTERFACE_HPP_ diff --git a/extern/bullet/src/BulletInverseDynamics/details/IDMatVec.hpp b/extern/bullet/src/BulletInverseDynamics/details/IDMatVec.hpp new file mode 100644 index 000000000000..4d3f6c87e9e1 --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/details/IDMatVec.hpp @@ -0,0 +1,415 @@ +/// @file Built-In Matrix-Vector functions +#ifndef IDMATVEC_HPP_ +#define IDMATVEC_HPP_ + +#include + +#include "../IDConfig.hpp" +#define BT_ID_HAVE_MAT3X + +namespace btInverseDynamics { +class vec3; +class vecx; +class mat33; +class matxx; +class mat3x; + +/// This is a very basic implementation to enable stand-alone use of the library. +/// The implementation is not really optimized and misses many features that you would +/// want from a "fully featured" linear math library. +class vec3 { +public: + idScalar& operator()(int i) { return m_data[i]; } + const idScalar& operator()(int i) const { return m_data[i]; } + const int size() const { return 3; } + const vec3& operator=(const vec3& rhs); + const vec3& operator+=(const vec3& b); + const vec3& operator-=(const vec3& b); + vec3 cross(const vec3& b) const; + idScalar dot(const vec3& b) const; + + friend vec3 operator*(const mat33& a, const vec3& b); + friend vec3 operator*(const vec3& a, const idScalar& s); + friend vec3 operator*(const idScalar& s, const vec3& a); + + friend vec3 operator+(const vec3& a, const vec3& b); + friend vec3 operator-(const vec3& a, const vec3& b); + friend vec3 operator/(const vec3& a, const idScalar& s); + +private: + idScalar m_data[3]; +}; + +class mat33 { +public: + idScalar& operator()(int i, int j) { return m_data[3 * i + j]; } + const idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; } + const mat33& operator=(const mat33& rhs); + mat33 transpose() const; + const mat33& operator+=(const mat33& b); + const mat33& operator-=(const mat33& b); + + friend mat33 operator*(const mat33& a, const mat33& b); + friend vec3 operator*(const mat33& a, const vec3& b); + friend mat33 operator*(const mat33& a, const idScalar& s); + friend mat33 operator*(const idScalar& s, const mat33& a); + friend mat33 operator+(const mat33& a, const mat33& b); + friend mat33 operator-(const mat33& a, const mat33& b); + friend mat33 operator/(const mat33& a, const idScalar& s); + +private: + // layout is [0,1,2;3,4,5;6,7,8] + idScalar m_data[9]; +}; + +class vecx { +public: + vecx(int size) : m_size(size) { + m_data = static_cast(idMalloc(sizeof(idScalar) * size)); + } + ~vecx() { idFree(m_data); } + const vecx& operator=(const vecx& rhs); + idScalar& operator()(int i) { return m_data[i]; } + const idScalar& operator()(int i) const { return m_data[i]; } + const int& size() const { return m_size; } + + friend vecx operator*(const vecx& a, const idScalar& s); + friend vecx operator*(const idScalar& s, const vecx& a); + + friend vecx operator+(const vecx& a, const vecx& b); + friend vecx operator-(const vecx& a, const vecx& b); + friend vecx operator/(const vecx& a, const idScalar& s); + +private: + int m_size; + idScalar* m_data; +}; + +class matxx { +public: + matxx() { + m_data = 0x0; + m_cols=0; + m_rows=0; + } + matxx(int rows, int cols) : m_rows(rows), m_cols(cols) { + m_data = static_cast(idMalloc(sizeof(idScalar) * rows * cols)); + } + ~matxx() { idFree(m_data); } + idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; } + const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; } + const int& rows() const { return m_rows; } + const int& cols() const { return m_cols; } + +private: + int m_rows; + int m_cols; + idScalar* m_data; +}; + +class mat3x { +public: + mat3x() { + m_data = 0x0; + m_cols=0; + } + mat3x(const mat3x&rhs) { + m_cols=rhs.m_cols; + allocate(); + *this = rhs; + } + mat3x(int rows, int cols): m_cols(cols) { + allocate(); + }; + void operator=(const mat3x& rhs) { + if (m_cols != rhs.m_cols) { + error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); + abort(); + } + for(int i=0;i<3*m_cols;i++) { + m_data[i] = rhs.m_data[i]; + } + } + + ~mat3x() { + free(); + } + idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; } + const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; } + int rows() const { return m_rows; } + const int& cols() const { return m_cols; } + void resize(int rows, int cols) { + m_cols=cols; + free(); + allocate(); + } + void setZero() { + memset(m_data,0x0,sizeof(idScalar)*m_rows*m_cols); + } + // avoid operators that would allocate -- use functions sub/add/mul in IDMath.hpp instead +private: + void allocate(){m_data = static_cast(idMalloc(sizeof(idScalar) * m_rows * m_cols));} + void free() { idFree(m_data);} + enum {m_rows=3}; + int m_cols; + idScalar* m_data; +}; + +inline void resize(mat3x &m, idArrayIdx size) { + m.resize(3, size); + m.setZero(); +} + +////////////////////////////////////////////////// +// Implementations +inline const vec3& vec3::operator=(const vec3& rhs) { + if (&rhs != this) { + memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar)); + } + return *this; +} + +inline vec3 vec3::cross(const vec3& b) const { + vec3 result; + result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1]; + result.m_data[1] = m_data[2] * b.m_data[0] - m_data[0] * b.m_data[2]; + result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0]; + + return result; +} + +inline idScalar vec3::dot(const vec3& b) const { + return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2]; +} + +inline const mat33& mat33::operator=(const mat33& rhs) { + if (&rhs != this) { + memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar)); + } + return *this; +} +inline mat33 mat33::transpose() const { + mat33 result; + result.m_data[0] = m_data[0]; + result.m_data[1] = m_data[3]; + result.m_data[2] = m_data[6]; + result.m_data[3] = m_data[1]; + result.m_data[4] = m_data[4]; + result.m_data[5] = m_data[7]; + result.m_data[6] = m_data[2]; + result.m_data[7] = m_data[5]; + result.m_data[8] = m_data[8]; + + return result; +} + +inline mat33 operator*(const mat33& a, const mat33& b) { + mat33 result; + result.m_data[0] = + a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[3] + a.m_data[2] * b.m_data[6]; + result.m_data[1] = + a.m_data[0] * b.m_data[1] + a.m_data[1] * b.m_data[4] + a.m_data[2] * b.m_data[7]; + result.m_data[2] = + a.m_data[0] * b.m_data[2] + a.m_data[1] * b.m_data[5] + a.m_data[2] * b.m_data[8]; + result.m_data[3] = + a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[3] + a.m_data[5] * b.m_data[6]; + result.m_data[4] = + a.m_data[3] * b.m_data[1] + a.m_data[4] * b.m_data[4] + a.m_data[5] * b.m_data[7]; + result.m_data[5] = + a.m_data[3] * b.m_data[2] + a.m_data[4] * b.m_data[5] + a.m_data[5] * b.m_data[8]; + result.m_data[6] = + a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[3] + a.m_data[8] * b.m_data[6]; + result.m_data[7] = + a.m_data[6] * b.m_data[1] + a.m_data[7] * b.m_data[4] + a.m_data[8] * b.m_data[7]; + result.m_data[8] = + a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8]; + + return result; +} + +inline const mat33& mat33::operator+=(const mat33& b) { + for (int i = 0; i < 9; i++) { + m_data[i] += b.m_data[i]; + } + + return *this; +} + +inline const mat33& mat33::operator-=(const mat33& b) { + for (int i = 0; i < 9; i++) { + m_data[i] -= b.m_data[i]; + } + return *this; +} + +inline vec3 operator*(const mat33& a, const vec3& b) { + vec3 result; + + result.m_data[0] = + a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[1] + a.m_data[2] * b.m_data[2]; + result.m_data[1] = + a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[1] + a.m_data[5] * b.m_data[2]; + result.m_data[2] = + a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2]; + + return result; +} + +inline const vec3& vec3::operator+=(const vec3& b) { + for (int i = 0; i < 3; i++) { + m_data[i] += b.m_data[i]; + } + return *this; +} + +inline const vec3& vec3::operator-=(const vec3& b) { + for (int i = 0; i < 3; i++) { + m_data[i] -= b.m_data[i]; + } + return *this; +} + +inline mat33 operator*(const mat33& a, const idScalar& s) { + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] * s; + } + return result; +} + +inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } + +inline vec3 operator*(const vec3& a, const idScalar& s) { + vec3 result; + for (int i = 0; i < 3; i++) { + result.m_data[i] = a.m_data[i] * s; + } + return result; +} +inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; } + +inline mat33 operator+(const mat33& a, const mat33& b) { + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] + b.m_data[i]; + } + return result; +} +inline vec3 operator+(const vec3& a, const vec3& b) { + vec3 result; + for (int i = 0; i < 3; i++) { + result.m_data[i] = a.m_data[i] + b.m_data[i]; + } + return result; +} + +inline mat33 operator-(const mat33& a, const mat33& b) { + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] - b.m_data[i]; + } + return result; +} +inline vec3 operator-(const vec3& a, const vec3& b) { + vec3 result; + for (int i = 0; i < 3; i++) { + result.m_data[i] = a.m_data[i] - b.m_data[i]; + } + return result; +} + +inline mat33 operator/(const mat33& a, const idScalar& s) { + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] / s; + } + return result; +} + +inline vec3 operator/(const vec3& a, const idScalar& s) { + vec3 result; + for (int i = 0; i < 3; i++) { + result.m_data[i] = a.m_data[i] / s; + } + return result; +} + +inline const vecx& vecx::operator=(const vecx& rhs) { + if (size() != rhs.size()) { + error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size()); + abort(); + } + if (&rhs != this) { + memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar)); + } + return *this; +} +inline vecx operator*(const vecx& a, const idScalar& s) { + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result.m_data[i] = a.m_data[i] * s; + } + return result; +} +inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; } +inline vecx operator+(const vecx& a, const vecx& b) { + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result.m_data[i] = a.m_data[i] + b.m_data[i]; + } + + return result; +} +inline vecx operator-(const vecx& a, const vecx& b) { + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result.m_data[i] = a.m_data[i] - b.m_data[i]; + } + return result; +} +inline vecx operator/(const vecx& a, const idScalar& s) { + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result.m_data[i] = a.m_data[i] / s; + } + + return result; +} + +inline vec3 operator*(const mat3x& a, const vecx& b) { + vec3 result; + if (a.cols() != b.size()) { + error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size()); + abort(); + } + result(0)=0.0; + result(1)=0.0; + result(2)=0.0; + for(int i=0;i(i)); + id_printf("type: %s\n", jointTypeToString(body.m_joint_type)); + id_printf("q_index= %d\n", body.m_q_index); + id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2)); + id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2)); + + id_printf("mass = %f\n", body.m_mass); + id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1), + body.m_body_mass_com(2)); + id_printf("I_o= [%f %f %f;\n" + " %f %f %f;\n" + " %f %f %f]\n", + body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2), + body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2), + body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2)); + + id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0), + body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2)); + } +} +int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const { + switch (type) { + case FIXED: + return 0; + case REVOLUTE: + case PRISMATIC: + return 1; + case FLOATING: + return 6; + } + error_message("unknown joint type %d\n", type); + return 0; +} + +void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { + // this is adapted from URDF2Bullet. + // TODO: fix this and print proper graph (similar to git --log --graph) + int num_children = m_child_indices[index].size(); + + indentation += 2; + int count = 0; + + for (int i = 0; i < num_children; i++) { + int child_index = m_child_indices[index][i]; + indent(indentation); + id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index, + jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1, + m_body_list[index].m_q_index, + m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type)); + // first grandchild + printTree(child_index, indentation); + } +} + +int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) { + m_world_gravity = gravity; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::generateIndexSets() { + m_body_revolute_list.resize(0); + m_body_prismatic_list.resize(0); + int q_index = 0; + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + body.m_q_index = -1; + switch (body.m_joint_type) { + case REVOLUTE: + m_body_revolute_list.push_back(i); + body.m_q_index = q_index; + q_index++; + break; + case PRISMATIC: + m_body_prismatic_list.push_back(i); + body.m_q_index = q_index; + q_index++; + break; + case FIXED: + // do nothing + break; + case FLOATING: + m_body_floating_list.push_back(i); + body.m_q_index = q_index; + q_index += 6; + break; + default: + error_message("unsupported joint type %d\n", body.m_joint_type); + return -1; + } + } + // sanity check + if (q_index != m_num_dofs) { + error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs); + return -1; + } + + m_child_indices.resize(m_body_list.size()); + + for (idArrayIdx child = 1; child < m_parent_index.size(); child++) { + const int &parent = m_parent_index[child]; + if (parent >= 0 && parent < (static_cast(m_parent_index.size()) - 1)) { + m_child_indices[parent].push_back(child); + } else { + if (-1 == parent) { + // multiple bodies are directly linked to the environment, ie, not a single root + error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); + } else { + // should never happen + error_message( + "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", + child, parent, static_cast(m_parent_index.size())); + } + return -1; + } + } + + return 0; +} + +void MultiBodyTree::MultiBodyImpl::calculateStaticData() { + // relative kinematics that are not a function of q, u, dot_u + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + switch (body.m_joint_type) { + case REVOLUTE: + body.m_parent_vel_rel(0) = 0; + body.m_parent_vel_rel(1) = 0; + body.m_parent_vel_rel(2) = 0; + body.m_parent_acc_rel(0) = 0; + body.m_parent_acc_rel(1) = 0; + body.m_parent_acc_rel(2) = 0; + body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; + break; + case PRISMATIC: + body.m_body_T_parent = body.m_body_T_parent_ref; + body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT; + body.m_body_ang_vel_rel(0) = 0; + body.m_body_ang_vel_rel(1) = 0; + body.m_body_ang_vel_rel(2) = 0; + body.m_body_ang_acc_rel(0) = 0; + body.m_body_ang_acc_rel(1) = 0; + body.m_body_ang_acc_rel(2) = 0; + break; + case FIXED: + body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; + body.m_body_T_parent = body.m_body_T_parent_ref; + body.m_body_ang_vel_rel(0) = 0; + body.m_body_ang_vel_rel(1) = 0; + body.m_body_ang_vel_rel(2) = 0; + body.m_parent_vel_rel(0) = 0; + body.m_parent_vel_rel(1) = 0; + body.m_parent_vel_rel(2) = 0; + body.m_body_ang_acc_rel(0) = 0; + body.m_body_ang_acc_rel(1) = 0; + body.m_body_ang_acc_rel(2) = 0; + body.m_parent_acc_rel(0) = 0; + body.m_parent_acc_rel(1) = 0; + body.m_parent_acc_rel(2) = 0; + break; + case FLOATING: + // no static data + break; + } + + // resize & initialize jacobians to zero. +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) + body.m_body_dot_Jac_T_u(0) = 0.0; + body.m_body_dot_Jac_T_u(1) = 0.0; + body.m_body_dot_Jac_T_u(2) = 0.0; + body.m_body_dot_Jac_R_u(0) = 0.0; + body.m_body_dot_Jac_R_u(1) = 0.0; + body.m_body_dot_Jac_R_u(2) = 0.0; + resize(body.m_body_Jac_T,m_num_dofs); + resize(body.m_body_Jac_R,m_num_dofs); + body.m_body_Jac_T.setZero(); + body.m_body_Jac_R.setZero(); +#endif // + } +} + +int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u, + const vecx &dot_u, vecx *joint_forces) { + if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs || + joint_forces->size() != m_num_dofs) { + error_message("wrong vector dimension. system has %d DOFs,\n" + "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n", + m_num_dofs, static_cast(q.size()), static_cast(u.size()), + static_cast(dot_u.size()), static_cast(joint_forces->size())); + return -1; + } + // 1. relative kinematics + if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) { + error_message("error in calculateKinematics\n"); + return -1; + } + // 2. update contributions to equations of motion for every body. + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + // 3.4 update dynamic terms (rate of change of angular & linear momentum) + body.m_eom_lhs_rotational = + body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) + + body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) - + body.m_body_moment_user; + body.m_eom_lhs_translational = + body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc + + body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) - + body.m_body_force_user; + } + + // 3. calculate full set of forces at parent joint + // (not directly calculating the joint force along the free direction + // simplifies inclusion of fixed joints. + // An alternative would be to fuse bodies in a pre-processing step, + // but that would make changing masses online harder (eg, payload masses + // added with fixed joints to a gripper) + // Also, this enables adding zero weight bodies as a way to calculate frame poses + // for force elements, etc. + + for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) { + // sum of forces and moments acting on this body from its children + vec3 sum_f_children; + vec3 sum_m_children; + setZero(sum_f_children); + setZero(sum_m_children); + for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size(); + child_list_idx++) { + const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]]; + vec3 child_joint_force_in_this_frame = + child.m_body_T_parent.transpose() * child.m_force_at_joint; + sum_f_children -= child_joint_force_in_this_frame; + sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint + + child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame); + } + RigidBody &body = m_body_list[body_idx]; + + body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children; + body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children; + } + + // 4. Calculate Joint forces. + // These are the components of force_at_joint/moment_at_joint + // in the free directions given by Jac_JT/Jac_JR + // 4.1 revolute joints + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + RigidBody &body = m_body_list[m_body_revolute_list[i]]; + // (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint; + (*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint); + } + // 4.2 for prismatic joints + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + RigidBody &body = m_body_list[m_body_prismatic_list[i]]; + // (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint; + (*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint); + } + // 4.3 floating bodies (6-DoF joints) + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + RigidBody &body = m_body_list[m_body_floating_list[i]]; + (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0); + (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1); + (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2); + + (*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0); + (*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1); + (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2); + } + + return 0; +} + +int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx& dot_u, + const KinUpdateType type) { + if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ) { + error_message("wrong vector dimension. system has %d DOFs,\n" + "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n", + m_num_dofs, static_cast(q.size()), static_cast(u.size()), + static_cast(dot_u.size())); + return -1; + } + if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) { + error_message("invalid type %d\n", type); + return -1; + } + + // 1. update relative kinematics + // 1.1 for revolute + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + RigidBody &body = m_body_list[m_body_revolute_list[i]]; + mat33 T; + bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T); + body.m_body_T_parent = T * body.m_body_T_parent_ref; + if(type >= POSITION_VELOCITY) { + body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index); + } + if(type >= POSITION_VELOCITY_ACCELERATION) { + body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index); + } + } + // 1.2 for prismatic + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + RigidBody &body = m_body_list[m_body_prismatic_list[i]]; + body.m_parent_pos_parent_body = + body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); + if(type >= POSITION_VELOCITY) { + body.m_parent_vel_rel = + body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index); + } + if(type >= POSITION_VELOCITY_ACCELERATION) { + body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index); + } + } + // 1.3 fixed joints: nothing to do + // 1.4 6dof joints: + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + RigidBody &body = m_body_list[m_body_floating_list[i]]; + + body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * + transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index)); + body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); + body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); + body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; + + if(type >= POSITION_VELOCITY) { + body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0); + body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1); + body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2); + + body.m_parent_vel_rel(0) = u(body.m_q_index + 3); + body.m_parent_vel_rel(1) = u(body.m_q_index + 4); + body.m_parent_vel_rel(2) = u(body.m_q_index + 5); + + body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel; + } + if(type >= POSITION_VELOCITY_ACCELERATION) { + body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0); + body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1); + body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2); + + body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3); + body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4); + body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5); + + body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; + } + } + + // 2. absolute kinematic quantities (vector valued) + // NOTE: this should be optimized by specializing for different body types + // (e.g., relative rotation is always zero for prismatic joints, etc.) + + // calculations for root body + { + RigidBody &body = m_body_list[0]; + // 3.1 update absolute positions and orientations: + // will be required if we add force elements (eg springs between bodies, + // or contacts) + // not required right now, added here for debugging purposes + body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body; + body.m_body_T_world = body.m_body_T_parent; + + if(type >= POSITION_VELOCITY) { + // 3.2 update absolute velocities + body.m_body_ang_vel = body.m_body_ang_vel_rel; + body.m_body_vel = body.m_parent_vel_rel; + } + if(type >= POSITION_VELOCITY_ACCELERATION) { + // 3.3 update absolute accelerations + // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints + body.m_body_ang_acc = body.m_body_ang_acc_rel; + body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel; + // add gravitational acceleration to root body + // this is an efficient way to add gravitational terms, + // but it does mean that the kinematics are no longer + // correct at the acceleration level + // NOTE: To get correct acceleration kinematics, just set world_gravity to zero + body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity; + } + } + + for (idArrayIdx i = 1; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + RigidBody &parent = m_body_list[m_parent_index[i]]; + // 2.1 update absolute positions and orientations: + // will be required if we add force elements (eg springs between bodies, + // or contacts) not required right now added here for debugging purposes + body.m_body_pos = + body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body); + body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world; + + if(type >= POSITION_VELOCITY) { + // 2.2 update absolute velocities + body.m_body_ang_vel = + body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel; + + body.m_body_vel = + body.m_body_T_parent * + (parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) + + body.m_parent_vel_rel); + } + if(type >= POSITION_VELOCITY_ACCELERATION) { + // 2.3 update absolute accelerations + // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints + body.m_body_ang_acc = + body.m_body_T_parent * parent.m_body_ang_acc - + body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) + + body.m_body_ang_acc_rel; + body.m_body_acc = + body.m_body_T_parent * + (parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) + + parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) + + 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel); + } + } + + return 0; +} + +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) + +void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody&body) { + const int& idx=body.m_q_index; + switch(body.m_joint_type) { + case FIXED: + break; + case REVOLUTE: + setMat3xElem(0,idx, body.m_Jac_JR(0), &body.m_body_Jac_R); + setMat3xElem(1,idx, body.m_Jac_JR(1), &body.m_body_Jac_R); + setMat3xElem(2,idx, body.m_Jac_JR(2), &body.m_body_Jac_R); + break; + case PRISMATIC: + setMat3xElem(0,idx, body.m_body_T_parent_ref(0,0)*body.m_Jac_JT(0) + +body.m_body_T_parent_ref(1,0)*body.m_Jac_JT(1) + +body.m_body_T_parent_ref(2,0)*body.m_Jac_JT(2), + &body.m_body_Jac_T); + setMat3xElem(1,idx,body.m_body_T_parent_ref(0,1)*body.m_Jac_JT(0) + +body.m_body_T_parent_ref(1,1)*body.m_Jac_JT(1) + +body.m_body_T_parent_ref(2,1)*body.m_Jac_JT(2), + &body.m_body_Jac_T); + setMat3xElem(2,idx, body.m_body_T_parent_ref(0,2)*body.m_Jac_JT(0) + +body.m_body_T_parent_ref(1,2)*body.m_Jac_JT(1) + +body.m_body_T_parent_ref(2,2)*body.m_Jac_JT(2), + &body.m_body_Jac_T); + break; + case FLOATING: + setMat3xElem(0,idx+0, 1.0, &body.m_body_Jac_R); + setMat3xElem(1,idx+1, 1.0, &body.m_body_Jac_R); + setMat3xElem(2,idx+2, 1.0, &body.m_body_Jac_R); + // body_Jac_T = body_T_parent.transpose(); + setMat3xElem(0,idx+3, body.m_body_T_parent(0,0), &body.m_body_Jac_T); + setMat3xElem(0,idx+4, body.m_body_T_parent(1,0), &body.m_body_Jac_T); + setMat3xElem(0,idx+5, body.m_body_T_parent(2,0), &body.m_body_Jac_T); + + setMat3xElem(1,idx+3, body.m_body_T_parent(0,1), &body.m_body_Jac_T); + setMat3xElem(1,idx+4, body.m_body_T_parent(1,1), &body.m_body_Jac_T); + setMat3xElem(1,idx+5, body.m_body_T_parent(2,1), &body.m_body_Jac_T); + + setMat3xElem(2,idx+3, body.m_body_T_parent(0,2), &body.m_body_Jac_T); + setMat3xElem(2,idx+4, body.m_body_T_parent(1,2), &body.m_body_Jac_T); + setMat3xElem(2,idx+5, body.m_body_T_parent(2,2), &body.m_body_Jac_T); + + break; + } +} + +int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) { + if (q.size() != m_num_dofs || u.size() != m_num_dofs) { + error_message("wrong vector dimension. system has %d DOFs,\n" + "but dim(q)= %d, dim(u)= %d\n", + m_num_dofs, static_cast(q.size()), static_cast(u.size())); + return -1; + } + if(type != POSITION_ONLY && type != POSITION_VELOCITY) { + error_message("invalid type %d\n", type); + return -1; + } + + addRelativeJacobianComponent(m_body_list[0]); + for (idArrayIdx i = 1; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + RigidBody &parent = m_body_list[m_parent_index[i]]; + + mul(body.m_body_T_parent, parent.m_body_Jac_R,& body.m_body_Jac_R); + body.m_body_Jac_T = parent.m_body_Jac_T; + mul(tildeOperator(body.m_parent_pos_parent_body),parent.m_body_Jac_R,&m_m3x); + sub(body.m_body_Jac_T,m_m3x, &body.m_body_Jac_T); + + addRelativeJacobianComponent(body); + mul(body.m_body_T_parent, body.m_body_Jac_T,&body.m_body_Jac_T); + + if(type >= POSITION_VELOCITY) { + body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u - + body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel); + body.m_body_dot_Jac_T_u = body.m_body_T_parent * + (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.cross(body.m_parent_pos_parent_body) + + parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) + + 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel)); + } + } + return 0; +} +#endif + +static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) { + switch (dof) { + // rotational part + case 0: + Jac_JR(0) = 1; + Jac_JR(1) = 0; + Jac_JR(2) = 0; + setZero(Jac_JT); + break; + case 1: + Jac_JR(0) = 0; + Jac_JR(1) = 1; + Jac_JR(2) = 0; + setZero(Jac_JT); + break; + case 2: + Jac_JR(0) = 0; + Jac_JR(1) = 0; + Jac_JR(2) = 1; + setZero(Jac_JT); + break; + // translational part + case 3: + setZero(Jac_JR); + Jac_JT(0) = 1; + Jac_JT(1) = 0; + Jac_JT(2) = 0; + break; + case 4: + setZero(Jac_JR); + Jac_JT(0) = 0; + Jac_JT(1) = 1; + Jac_JT(2) = 0; + break; + case 5: + setZero(Jac_JR); + Jac_JT(0) = 0; + Jac_JT(1) = 0; + Jac_JT(2) = 1; + break; + } +} + +static inline int jointNumDoFs(const JointType &type) { + switch (type) { + case FIXED: + return 0; + case REVOLUTE: + case PRISMATIC: + return 1; + case FLOATING: + return 6; + } + // this should never happen + error_message("invalid joint type\n"); + // TODO add configurable abort/crash function + abort(); + return 0; +} + +int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics, + const bool initialize_matrix, + const bool set_lower_triangular_matrix, + matxx *mass_matrix) { +// This calculates the joint space mass matrix for the multibody system. +// The algorithm is essentially an implementation of "method 3" +// in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982) +// (Later named "Composite Rigid Body Algorithm" by Featherstone). +// +// This implementation, however, handles branched systems and uses a formulation centered +// on the origin of the body-fixed frame to avoid re-computing various quantities at the com. + + if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs || + mass_matrix->cols() != m_num_dofs) { + error_message("Dimension error. System has %d DOFs,\n" + "but dim(q)= %d, dim(mass_matrix)= %d x %d\n", + m_num_dofs, static_cast(q.size()), static_cast(mass_matrix->rows()), + static_cast(mass_matrix->cols())); + return -1; + } + + // TODO add optimized zeroing function? + if (initialize_matrix) { + for (int i = 0; i < m_num_dofs; i++) { + for (int j = 0; j < m_num_dofs; j++) { + setMatxxElem(i, j, 0.0, mass_matrix); + } + } + } + + if (update_kinematics) { + // 1. update relative kinematics + // 1.1 for revolute joints + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + RigidBody &body = m_body_list[m_body_revolute_list[i]]; + // from reference orientation (q=0) of body-fixed frame to current orientation + mat33 body_T_body_ref; + bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref); + body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref; + } + // 1.2 for prismatic joints + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + RigidBody &body = m_body_list[m_body_prismatic_list[i]]; + // body.m_body_T_parent= fixed + body.m_parent_pos_parent_body = + body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); + } + // 1.3 fixed joints: nothing to do + // 1.4 6dof joints: + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + RigidBody &body = m_body_list[m_body_floating_list[i]]; + + body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * + transformY(q(body.m_q_index + 1)) * + transformX(q(body.m_q_index)); + body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); + body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); + body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); + + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; + } + } + for (int i = m_body_list.size() - 1; i >= 0; i--) { + RigidBody &body = m_body_list[i]; + // calculate mass, center of mass and inertia of "composite rigid body", + // ie, sub-tree starting at current body + body.m_subtree_mass = body.m_mass; + body.m_body_subtree_mass_com = body.m_body_mass_com; + body.m_body_subtree_I_body = body.m_body_I_body; + + for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++) { + RigidBody &child = m_body_list[m_child_indices[i][c]]; + mat33 body_T_child = child.m_body_T_parent.transpose(); + + body.m_subtree_mass += child.m_subtree_mass; + body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com + + child.m_parent_pos_parent_body * child.m_subtree_mass; + body.m_body_subtree_I_body += + body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent; + + if (child.m_subtree_mass > 0) { + // Shift the reference point for the child subtree inertia using the + // Huygens-Steiner ("parallel axis") theorem. + // (First shift from child origin to child com, then from there to this body's + // origin) + vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass; + mat33 tilde_r_child_com = tildeOperator(r_com); + mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com); + body.m_body_subtree_I_body += + child.m_subtree_mass * + (tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com); + } + } + } + + for (int i = m_body_list.size() - 1; i >= 0; i--) { + const RigidBody &body = m_body_list[i]; + + // determine DoF-range for body + const int q_index_min = body.m_q_index; + const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1; + // loop over the DoFs used by this body + // local joint jacobians (ok as is for 1-DoF joints) + vec3 Jac_JR = body.m_Jac_JR; + vec3 Jac_JT = body.m_Jac_JT; + for (int col = q_index_max; col >= q_index_min; col--) { + // set jacobians for 6-DoF joints + if (FLOATING == body.m_joint_type) { + setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT); + } + + vec3 body_eom_rot = + body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT); + vec3 body_eom_trans = + body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR); + setMatxxElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans), mass_matrix); + + // rest of the mass matrix column upwards + { + // 1. for multi-dof joints, rest of the dofs of this body + for (int row = col - 1; row >= q_index_min; row--) { + if (FLOATING != body.m_joint_type) { + error_message("??\n"); + return -1; + } + setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); + const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); + setMatxxElem(col, row, Mrc, mass_matrix); + } + // 2. ancestor dofs + int child_idx = i; + int parent_idx = m_parent_index[i]; + while (parent_idx >= 0) { + const RigidBody &child_body = m_body_list[child_idx]; + const RigidBody &parent_body = m_body_list[parent_idx]; + + const mat33 parent_T_child = child_body.m_body_T_parent.transpose(); + body_eom_rot = parent_T_child * body_eom_rot; + body_eom_trans = parent_T_child * body_eom_trans; + body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans); + + const int parent_body_q_index_min = parent_body.m_q_index; + const int parent_body_q_index_max = + parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1; + vec3 Jac_JR = parent_body.m_Jac_JR; + vec3 Jac_JT = parent_body.m_Jac_JT; + for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) { + // set jacobians for 6-DoF joints + if (FLOATING == parent_body.m_joint_type) { + setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT); + } + const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); + setMatxxElem(col, row, Mrc, mass_matrix); + } + + child_idx = parent_idx; + parent_idx = m_parent_index[child_idx]; + } + } + } + } + + if (set_lower_triangular_matrix) { + for (int col = 0; col < m_num_dofs; col++) { + for (int row = 0; row < col; row++) { + setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix); + } + } + } + return 0; +} + +// utility macro +#define CHECK_IF_BODY_INDEX_IS_VALID(index) \ + do { \ + if (index < 0 || index >= m_num_bodies) { \ + error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \ + return -1; \ + } \ + } while (0) + +int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *p = m_parent_index[body_index]; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *user_int = m_user_int[body_index]; + return 0; +} +int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *user_ptr = m_user_ptr[body_index]; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_user_int[body_index] = user_int; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_user_ptr[body_index] = user_ptr; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_origin = body.m_body_T_world.transpose() * body.m_body_pos; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + if (body.m_mass > 0) { + *world_com = body.m_body_T_world.transpose() * + (body.m_body_pos + body.m_body_mass_com / body.m_mass); + } else { + *world_com = body.m_body_T_world.transpose() * (body.m_body_pos); + } + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_T_body = body.m_body_T_world.transpose(); + return 0; +} +int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel; + return 0; +} +int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index, + vec3 *world_velocity) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_velocity = body.m_body_T_world.transpose() * body.m_body_vel; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index, + vec3 *world_velocity) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + vec3 com; + if (body.m_mass > 0) { + com = body.m_body_mass_com / body.m_mass; + } else { + com(0) = 0; + com(1) = 0; + com(2) = 0; + } + + *world_velocity = + body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com)); + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index, + vec3 *world_dot_omega) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc; + return 0; +} +int MultiBodyTree::MultiBodyImpl::getBodyLinearAcceleration(int body_index, + vec3 *world_acceleration) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *joint_type = m_body_list[body_index].m_joint_type; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index, + const char **joint_type) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *joint_type = jointTypeToString(m_body_list[body_index].m_joint_type); + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getParentRParentBodyRef(const int body_index, vec3* r) const{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *r=m_body_list[body_index].m_parent_pos_parent_body_ref; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33* T) const{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *T=m_body_list[body_index].m_body_T_parent_ref; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3* axis) const{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + if(m_body_list[body_index].m_joint_type == REVOLUTE) { + *axis = m_body_list[body_index].m_Jac_JR; + return 0; + } + if(m_body_list[body_index].m_joint_type == PRISMATIC) { + *axis = m_body_list[body_index].m_Jac_JT; + return 0; + } + setZero(*axis); + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *q_index = m_body_list[body_index].m_q_index; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_mass = mass; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index, + const vec3& first_mass_moment) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_mass_com = first_mass_moment; + return 0; +} +int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index, + const mat33& second_mass_moment) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_I_body = second_mass_moment; + return 0; +} +int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *mass = m_body_list[body_index].m_mass; + return 0; +} +int MultiBodyTree::MultiBodyImpl::getBodyFirstMassMoment(const int body_index, + vec3 *first_mass_moment) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *first_mass_moment = m_body_list[body_index].m_body_mass_com; + return 0; +} +int MultiBodyTree::MultiBodyImpl::getBodySecondMassMoment(const int body_index, + mat33 *second_mass_moment) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *second_mass_moment = m_body_list[body_index].m_body_I_body; + return 0; +} + +void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments() { + for (int index = 0; index < m_num_bodies; index++) { + RigidBody &body = m_body_list[index]; + setZero(body.m_body_force_user); + setZero(body.m_body_moment_user); + } +} + +int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_force_user += body_force; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_moment_user += body_moment; + return 0; +} + +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) +int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + mul(body.m_body_T_world.transpose(), body.m_body_Jac_T, world_jac_trans); + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const{ + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + mul(body.m_body_T_world.transpose(), body.m_body_Jac_R,world_jac_rot); + return 0; +} + +#endif +} diff --git a/extern/bullet/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/extern/bullet/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp new file mode 100644 index 000000000000..3efe9d049269 --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp @@ -0,0 +1,283 @@ +// The structs and classes defined here provide a basic inverse fynamics implementation used +// by MultiBodyTree +// User interaction should be through MultiBodyTree + +#ifndef MULTI_BODY_REFERENCE_IMPL_HPP_ +#define MULTI_BODY_REFERENCE_IMPL_HPP_ + +#include "../IDConfig.hpp" +#include "../MultiBodyTree.hpp" + +namespace btInverseDynamics { + +/// Structure for for rigid body mass properties, connectivity and kinematic state +/// all vectors and matrices are in body-fixed frame, if not indicated otherwise. +/// The body-fixed frame is located in the joint connecting the body to its parent. +struct RigidBody { + ID_DECLARE_ALIGNED_ALLOCATOR(); + // 1 Inertial properties + /// Mass + idScalar m_mass; + /// Mass times center of gravity in body-fixed frame + vec3 m_body_mass_com; + /// Moment of inertia w.r.t. body-fixed frame + mat33 m_body_I_body; + + // 2 dynamic properties + /// Left-hand side of the body equation of motion, translational part + vec3 m_eom_lhs_translational; + /// Left-hand side of the body equation of motion, rotational part + vec3 m_eom_lhs_rotational; + /// Force acting at the joint when the body is cut from its parent; + /// includes impressed joint force in J_JT direction, + /// as well as constraint force, + /// in body-fixed frame + vec3 m_force_at_joint; + /// Moment acting at the joint when the body is cut from its parent; + /// includes impressed joint moment in J_JR direction, and constraint moment + /// in body-fixed frame + vec3 m_moment_at_joint; + /// external (user provided) force acting at the body-fixed frame's origin, written in that + /// frame + vec3 m_body_force_user; + /// external (user provided) moment acting at the body-fixed frame's origin, written in that + /// frame + vec3 m_body_moment_user; + // 3 absolute kinematic properties + /// Position of body-fixed frame relative to world frame + /// this is currently only for debugging purposes + vec3 m_body_pos; + /// Absolute velocity of body-fixed frame + vec3 m_body_vel; + /// Absolute acceleration of body-fixed frame + /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational + /// acceleration! + vec3 m_body_acc; + /// Absolute angular velocity + vec3 m_body_ang_vel; + /// Absolute angular acceleration + /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational + /// acceleration! + vec3 m_body_ang_acc; + + // 4 relative kinematic properties. + // these are in the parent body frame + /// Transform from world to body-fixed frame; + /// this is currently only for debugging purposes + mat33 m_body_T_world; + /// Transform from parent to body-fixed frame + mat33 m_body_T_parent; + /// Vector from parent to child frame in parent frame + vec3 m_parent_pos_parent_body; + /// Relative angular velocity + vec3 m_body_ang_vel_rel; + /// Relative linear velocity + vec3 m_parent_vel_rel; + /// Relative angular acceleration + vec3 m_body_ang_acc_rel; + /// Relative linear acceleration + vec3 m_parent_acc_rel; + + // 5 Data describing the joint type and geometry + /// Type of joint + JointType m_joint_type; + /// Position of joint frame (body-fixed frame at q=0) relative to the parent frame + /// Components are in body-fixed frame of the parent + vec3 m_parent_pos_parent_body_ref; + /// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame + mat33 m_body_T_parent_ref; + /// Joint rotational Jacobian, ie, the partial derivative of the body-fixed frames absolute + /// angular velocity w.r.t. the generalized velocity of this body's relative degree of freedom. + /// For revolute joints this is the joint axis, for prismatic joints it is a null matrix. + /// (NOTE: dimensions will have to be dynamic for additional joint types!) + vec3 m_Jac_JR; + /// Joint translational Jacobian, ie, the partial derivative of the body-fixed frames absolute + /// linear velocity w.r.t. the generalized velocity of this body's relative degree of freedom. + /// For prismatic joints this is the joint axis, for revolute joints it is a null matrix. + /// (NOTE: dimensions might have to be dynamic for additional joint types!) + vec3 m_Jac_JT; + /// m_Jac_JT in the parent frame, it, m_body_T_parent_ref.transpose()*m_Jac_JT + vec3 m_parent_Jac_JT; + /// Start of index range for the position degree(s) of freedom describing this body's motion + /// relative to + /// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates. + int m_q_index; + + // 6 Scratch data for mass matrix computation using "composite rigid body algorithm" + /// mass of the subtree rooted in this body + idScalar m_subtree_mass; + /// center of mass * mass for subtree rooted in this body, in body-fixed frame + vec3 m_body_subtree_mass_com; + /// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame + mat33 m_body_subtree_I_body; + +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) + /// translational jacobian in body-fixed frame d(m_body_vel)/du + mat3x m_body_Jac_T; + /// rotationsl jacobian in body-fixed frame d(m_body_ang_vel)/du + mat3x m_body_Jac_R; + /// components of linear acceleration depending on u + /// (same as is d(m_Jac_T)/dt*u) + vec3 m_body_dot_Jac_T_u; + /// components of angular acceleration depending on u + /// (same as is d(m_Jac_T)/dt*u) + vec3 m_body_dot_Jac_R_u; +#endif +}; + +/// The MBS implements a tree structured multibody system +class MultiBodyTree::MultiBodyImpl { + friend class MultiBodyTree; + +public: + ID_DECLARE_ALIGNED_ALLOCATOR(); + + enum KinUpdateType { + POSITION_ONLY, + POSITION_VELOCITY, + POSITION_VELOCITY_ACCELERATION + }; + + /// constructor + /// @param num_bodies the number of bodies in the system + /// @param num_dofs number of degrees of freedom in the system + MultiBodyImpl(int num_bodies_, int num_dofs_); + + /// \copydoc MultiBodyTree::calculateInverseDynamics + int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, + vecx* joint_forces); + ///\copydoc MultiBodyTree::calculateMassMatrix + int calculateMassMatrix(const vecx& q, const bool update_kinematics, + const bool initialize_matrix, const bool set_lower_triangular_matrix, + matxx* mass_matrix); + /// calculate kinematics (vector quantities) + /// Depending on type, update positions only, positions & velocities, or positions, velocities + /// and accelerations. + int calculateKinematics(const vecx& q, const vecx& u, const vecx& dot_u, const KinUpdateType type); +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) + /// calculate jacobians and (if type == POSITION_VELOCITY), also velocity-dependent accelration terms. + int calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type); + /// \copydoc MultiBodyTree::getBodyDotJacobianTransU + int getBodyDotJacobianTransU(const int body_index, vec3* world_dot_jac_trans_u) const ; + /// \copydoc MultiBodyTree::getBodyDotJacobianRotU + int getBodyDotJacobianRotU(const int body_index, vec3* world_dot_jac_rot_u) const; + /// \copydoc MultiBodyTree::getBodyJacobianTrans + int getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const ; + /// \copydoc MultiBodyTree::getBodyJacobianRot + int getBodyJacobianRot(const int body_index, mat3x* world_jac_rot) const; + /// Add relative Jacobian component from motion relative to parent body + /// @param body the body to add the Jacobian component for + void addRelativeJacobianComponent(RigidBody&body); +#endif + /// generate additional index sets from the parent_index array + /// @return -1 on error, 0 on success + int generateIndexSets(); + /// set gravity acceleration in world frame + /// @param gravity gravity vector in the world frame + /// @return 0 on success, -1 on error + int setGravityInWorldFrame(const vec3& gravity); + /// pretty print tree + void printTree(); + /// print tree data + void printTreeData(); + /// initialize fixed data + void calculateStaticData(); + /// \copydoc MultiBodyTree::getBodyFrame + int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const; + /// \copydoc MultiBodyTree::getParentIndex + int getParentIndex(const int body_index, int* m_parent_index); + /// \copydoc MultiBodyTree::getJointType + int getJointType(const int body_index, JointType* joint_type) const; + /// \copydoc MultiBodyTree::getJointTypeStr + int getJointTypeStr(const int body_index, const char** joint_type) const; + /// \copydoc MultiBodyTree::getParentRParentBodyRef + int getParentRParentBodyRef(const int body_index, vec3* r) const; + /// \copydoc MultiBodyTree::getBodyTParentRef + int getBodyTParentRef(const int body_index, mat33* T) const; + /// \copydoc MultiBodyTree::getBodyAxisOfMotion + int getBodyAxisOfMotion(const int body_index, vec3* axis) const; + /// \copydoc MultiBodyTree:getDoFOffset + int getDoFOffset(const int body_index, int* q_index) const; + /// \copydoc MultiBodyTree::getBodyOrigin + int getBodyOrigin(const int body_index, vec3* world_origin) const; + /// \copydoc MultiBodyTree::getBodyCoM + int getBodyCoM(const int body_index, vec3* world_com) const; + /// \copydoc MultiBodyTree::getBodyTransform + int getBodyTransform(const int body_index, mat33* world_T_body) const; + /// \copydoc MultiBodyTree::getBodyAngularVelocity + int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; + /// \copydoc MultiBodyTree::getBodyLinearVelocity + int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; + /// \copydoc MultiBodyTree::getBodyLinearVelocityCoM + int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; + /// \copydoc MultiBodyTree::getBodyAngularAcceleration + int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; + /// \copydoc MultiBodyTree::getBodyLinearAcceleration + int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; + /// \copydoc MultiBodyTree::getUserInt + int getUserInt(const int body_index, int* user_int) const; + /// \copydoc MultiBodyTree::getUserPtr + int getUserPtr(const int body_index, void** user_ptr) const; + /// \copydoc MultiBodyTree::setUserInt + int setUserInt(const int body_index, const int user_int); + /// \copydoc MultiBodyTree::setUserPtr + int setUserPtr(const int body_index, void* const user_ptr); + ///\copydoc MultiBodytTree::setBodyMass + int setBodyMass(const int body_index, const idScalar mass); + ///\copydoc MultiBodytTree::setBodyFirstMassMoment + int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); + ///\copydoc MultiBodytTree::setBodySecondMassMoment + int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); + ///\copydoc MultiBodytTree::getBodyMass + int getBodyMass(const int body_index, idScalar* mass) const; + ///\copydoc MultiBodytTree::getBodyFirstMassMoment + int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; + ///\copydoc MultiBodytTree::getBodySecondMassMoment + int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; + /// \copydoc MultiBodyTree::clearAllUserForcesAndMoments + void clearAllUserForcesAndMoments(); + /// \copydoc MultiBodyTree::addUserForce + int addUserForce(const int body_index, const vec3& body_force); + /// \copydoc MultiBodyTree::addUserMoment + int addUserMoment(const int body_index, const vec3& body_moment); + +private: + // debug function. print tree structure to stdout + void printTree(int index, int indentation); + // get string representation of JointType (for debugging) + const char* jointTypeToString(const JointType& type) const; + // get number of degrees of freedom from joint type + int bodyNumDoFs(const JointType& type) const; + // number of bodies in the system + int m_num_bodies; + // number of degrees of freedom + int m_num_dofs; + // Gravitational acceleration (in world frame) + vec3 m_world_gravity; + // vector of bodies in the system + // body 0 is used as an environment body and is allways fixed. + // The bodies are ordered such that a parent body always has an index + // smaller than its child. + idArray::type m_body_list; + // Parent_index[i] is the index for i's parent body in body_list. + // This fully describes the tree. + idArray::type m_parent_index; + // child_indices[i] contains a vector of indices of + // all children of the i-th body + idArray::type>::type m_child_indices; + // Indices of rotary joints + idArray::type m_body_revolute_list; + // Indices of prismatic joints + idArray::type m_body_prismatic_list; + // Indices of floating joints + idArray::type m_body_floating_list; + // a user-provided integer + idArray::type m_user_int; + // a user-provided pointer + idArray::type m_user_ptr; +#if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS) + mat3x m_m3x; +#endif +}; +} +#endif diff --git a/extern/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/extern/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp new file mode 100644 index 000000000000..47b4ab38908e --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp @@ -0,0 +1,113 @@ +#include "MultiBodyTreeInitCache.hpp" + +namespace btInverseDynamics { + +MultiBodyTree::InitCache::InitCache() { + m_inertias.resize(0); + m_joints.resize(0); + m_num_dofs = 0; + m_root_index=-1; +} + +int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index, + const JointType joint_type, + const vec3& parent_r_parent_body_ref, + const mat33& body_T_parent_ref, + const vec3& body_axis_of_motion, const idScalar mass, + const vec3& body_r_body_com, const mat33& body_I_body, + const int user_int, void* user_ptr) { + switch (joint_type) { + case REVOLUTE: + case PRISMATIC: + m_num_dofs += 1; + break; + case FIXED: + // does not add a degree of freedom + // m_num_dofs+=0; + break; + case FLOATING: + m_num_dofs += 6; + break; + default: + error_message("unknown joint type %d\n", joint_type); + return -1; + } + + if(-1 == parent_index) { + if(m_root_index>=0) { + error_message("trying to add body %d as root, but already added %d as root body\n", + body_index, m_root_index); + return -1; + } + m_root_index=body_index; + } + + JointData joint; + joint.m_child = body_index; + joint.m_parent = parent_index; + joint.m_type = joint_type; + joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref; + joint.m_child_T_parent_ref = body_T_parent_ref; + joint.m_child_axis_of_motion = body_axis_of_motion; + + InertiaData body; + body.m_mass = mass; + body.m_body_pos_body_com = body_r_body_com; + body.m_body_I_body = body_I_body; + + m_inertias.push_back(body); + m_joints.push_back(joint); + m_user_int.push_back(user_int); + m_user_ptr.push_back(user_ptr); + return 0; +} +int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const { + if (index < 0 || index > static_cast(m_inertias.size())) { + error_message("index out of range\n"); + return -1; + } + + *inertia = m_inertias[index]; + return 0; +} + +int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const { + if (index < 0 || index > static_cast(m_user_int.size())) { + error_message("index out of range\n"); + return -1; + } + *user_int = m_user_int[index]; + return 0; +} + +int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const { + if (index < 0 || index > static_cast(m_user_ptr.size())) { + error_message("index out of range\n"); + return -1; + } + *user_ptr = m_user_ptr[index]; + return 0; +} + +int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const { + if (index < 0 || index > static_cast(m_joints.size())) { + error_message("index out of range\n"); + return -1; + } + *joint = m_joints[index]; + return 0; +} + +int MultiBodyTree::InitCache::buildIndexSets() { + // NOTE: This function assumes that proper indices were provided + // User2InternalIndex from utils can be used to facilitate this. + + m_parent_index.resize(numBodies()); + for (idArrayIdx j = 0; j < m_joints.size(); j++) { + const JointData& joint = m_joints[j]; + m_parent_index[joint.m_child] = joint.m_parent; + } + + return 0; +} +} diff --git a/extern/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp b/extern/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp new file mode 100644 index 000000000000..0d2aa4a07184 --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp @@ -0,0 +1,109 @@ +#ifndef MULTIBODYTREEINITCACHE_HPP_ +#define MULTIBODYTREEINITCACHE_HPP_ + +#include "../IDConfig.hpp" +#include "../IDMath.hpp" +#include "../MultiBodyTree.hpp" + +namespace btInverseDynamics { +/// Mass properties of a rigid body +struct InertiaData { + ID_DECLARE_ALIGNED_ALLOCATOR(); + + /// mass + idScalar m_mass; + /// vector from body-fixed frame to center of mass, + /// in body-fixed frame, multiplied by the mass + vec3 m_body_pos_body_com; + /// moment of inertia w.r.t. the origin of the body-fixed + /// frame, represented in that frame + mat33 m_body_I_body; +}; + +/// Joint properties +struct JointData { + ID_DECLARE_ALIGNED_ALLOCATOR(); + + /// type of joint + JointType m_type; + /// index of parent body + int m_parent; + /// index of child body + int m_child; + /// vector from parent's body-fixed frame to child's body-fixed + /// frame for q=0, written in the parent's body fixed frame + vec3 m_parent_pos_parent_child_ref; + /// Transform matrix converting vectors written in the parent's frame + /// into vectors written in the child's frame for q=0 + /// ie, child_vector = child_T_parent_ref * parent_vector; + mat33 m_child_T_parent_ref; + /// Axis of motion for 1 degree-of-freedom joints, + /// written in the child's frame + /// For revolute joints, the q-value is positive for a positive + /// rotation about this axis. + /// For prismatic joints, the q-value is positive for a positive + /// translation is this direction. + vec3 m_child_axis_of_motion; +}; + +/// Data structure to store data passed by the user. +/// This is used in MultiBodyTree::finalize to build internal data structures. +class MultiBodyTree::InitCache { +public: + ID_DECLARE_ALIGNED_ALLOCATOR(); + /// constructor + InitCache(); + ///\copydoc MultiBodyTree::addBody + int addBody(const int body_index, const int parent_index, const JointType joint_type, + const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref, + const vec3 &body_axis_of_motion, idScalar mass, const vec3 &body_r_body_com, + const mat33 &body_I_body, const int user_int, void *user_ptr); + /// build index arrays + /// @return 0 on success, -1 on failure + int buildIndexSets(); + /// @return number of degrees of freedom + int numDoFs() const { return m_num_dofs; } + /// @return number of bodies + int numBodies() const { return m_inertias.size(); } + /// get inertia data for index + /// @param index of the body + /// @param inertia pointer for return data + /// @return 0 on success, -1 on failure + int getInertiaData(const int index, InertiaData *inertia) const; + /// get joint data for index + /// @param index of the body + /// @param joint pointer for return data + /// @return 0 on success, -1 on failure + int getJointData(const int index, JointData *joint) const; + /// get parent index array (paren_index[i] is the index of the parent of i) + /// @param parent_index pointer for return data + void getParentIndexArray(idArray::type *parent_index) { *parent_index = m_parent_index; } + /// get user integer + /// @param index body index + /// @param user_int user integer + /// @return 0 on success, -1 on failure + int getUserInt(const int index, int *user_int) const; + /// get user pointer + /// @param index body index + /// @param user_int user pointer + /// @return 0 on success, -1 on failure + int getUserPtr(const int index, void **user_ptr) const; + +private: + // vector of bodies + idArray::type m_inertias; + // vector of joints + idArray::type m_joints; + // number of mechanical degrees of freedom + int m_num_dofs; + // parent index array + idArray::type m_parent_index; + // user integers + idArray::type m_user_int; + // user pointers + idArray::type m_user_ptr; + // index of root body (or -1 if not set) + int m_root_index; +}; +} +#endif // MULTIBODYTREEINITCACHE_HPP_ diff --git a/extern/bullet/src/BulletInverseDynamics/premake4.lua b/extern/bullet/src/BulletInverseDynamics/premake4.lua new file mode 100644 index 000000000000..fdd176b09f78 --- /dev/null +++ b/extern/bullet/src/BulletInverseDynamics/premake4.lua @@ -0,0 +1,15 @@ + project "BulletInverseDynamics" + + kind "StaticLib" + if os.is("Linux") then + buildoptions{"-fPIC"} + end + includedirs { + "..", + } + files { + "IDMath.cpp", + "MultiBodyTree.cpp", + "details/MultiBodyTreeInitCache.cpp", + "details/MultiBodyTreeImpl.cpp", + } diff --git a/extern/bullet/src/BulletSoftBody/CMakeLists.txt b/extern/bullet/src/BulletSoftBody/CMakeLists.txt new file mode 100644 index 000000000000..d43df1c67b31 --- /dev/null +++ b/extern/bullet/src/BulletSoftBody/CMakeLists.txt @@ -0,0 +1,69 @@ + +INCLUDE_DIRECTORIES( +${BULLET_PHYSICS_SOURCE_DIR}/src + +) + +#SUBDIRS( Solvers ) + +SET(BulletSoftBody_SRCS + btSoftBody.cpp + btSoftBodyConcaveCollisionAlgorithm.cpp + btSoftBodyHelpers.cpp + btSoftBodyRigidBodyCollisionConfiguration.cpp + btSoftRigidCollisionAlgorithm.cpp + btSoftRigidDynamicsWorld.cpp + btSoftMultiBodyDynamicsWorld.cpp + btSoftSoftCollisionAlgorithm.cpp + btDefaultSoftBodySolver.cpp + +) + +SET(BulletSoftBody_HDRS + btSoftBody.h + btSoftBodyData.h + btSoftBodyConcaveCollisionAlgorithm.h + btSoftBodyHelpers.h + btSoftBodyRigidBodyCollisionConfiguration.h + btSoftRigidCollisionAlgorithm.h + btSoftRigidDynamicsWorld.h + btSoftMultiBodyDynamicsWorld.h + btSoftSoftCollisionAlgorithm.h + btSparseSDF.h + + btSoftBodySolvers.h + btDefaultSoftBodySolver.h + + btSoftBodySolverVertexBuffer.h +) + + + +ADD_LIBRARY(BulletSoftBody ${BulletSoftBody_SRCS} ${BulletSoftBody_HDRS}) +SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES SOVERSION ${BULLET_VERSION}) +IF (BUILD_SHARED_LIBS) + TARGET_LINK_LIBRARIES(BulletSoftBody BulletDynamics) +ENDIF (BUILD_SHARED_LIBS) + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletSoftBody DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS BulletSoftBody RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN +".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(BulletSoftBody PROPERTIES PUBLIC_HEADER "${BulletSoftBody_HDRS}") + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp b/extern/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.cpp similarity index 94% rename from extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp rename to extern/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.cpp index e90d24e6edfb..9c2040307437 100644 --- a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp +++ b/extern/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.cpp @@ -100,9 +100,9 @@ void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *cons for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) { btVector3 position = clothVertices[vertexIndex].m_x; - *(vertexPointer + 0) = position.getX(); - *(vertexPointer + 1) = position.getY(); - *(vertexPointer + 2) = position.getZ(); + *(vertexPointer + 0) = (float)position.getX(); + *(vertexPointer + 1) = (float)position.getY(); + *(vertexPointer + 2) = (float)position.getZ(); vertexPointer += vertexStride; } } @@ -115,9 +115,9 @@ void btDefaultSoftBodySolver::copySoftBodyToVertexBuffer( const btSoftBody *cons for( int vertexIndex = 0; vertexIndex < numVertices; ++vertexIndex ) { btVector3 normal = clothVertices[vertexIndex].m_n; - *(normalPointer + 0) = normal.getX(); - *(normalPointer + 1) = normal.getY(); - *(normalPointer + 2) = normal.getZ(); + *(normalPointer + 0) = (float)normal.getX(); + *(normalPointer + 1) = (float)normal.getY(); + *(normalPointer + 2) = (float)normal.getZ(); normalPointer += normalStride; } } diff --git a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h b/extern/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.h similarity index 100% rename from extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h rename to extern/bullet/src/BulletSoftBody/btDefaultSoftBodySolver.h diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet/src/BulletSoftBody/btSoftBody.cpp similarity index 96% rename from extern/bullet2/src/BulletSoftBody/btSoftBody.cpp rename to extern/bullet/src/BulletSoftBody/btSoftBody.cpp index 51f4b33d034d..48efb0d8d48b 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp +++ b/extern/bullet/src/BulletSoftBody/btSoftBody.cpp @@ -18,6 +18,8 @@ subject to the following restrictions: #include "BulletSoftBody/btSoftBodySolvers.h" #include "btSoftBodyData.h" #include "LinearMath/btSerializer.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" // @@ -522,7 +524,7 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeInde } else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided) { - if (btSoftBody::eAeroModel::V_TwoSided) + if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided) nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); const btScalar dvn = btDot(rel_v,nrm); @@ -617,7 +619,7 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceInde } else if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided) { - if (btSoftBody::eAeroModel::F_TwoSided) + if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided) nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); const btScalar dvn=btDot(rel_v,nrm); @@ -3001,6 +3003,7 @@ void btSoftBody::applyForces() // void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti) { + BT_PROFILE("PSolve_Anchors"); const btScalar kAHR=psb->m_cfg.kAHR*kst; const btScalar dt=psb->m_sst.sdt; for(int i=0,ni=psb->m_anchors.size();im_sst.sdt; const btScalar mrg = psb->getCollisionShape()->getMargin(); + btMultiBodyJacobianData jacobianData; for(int i=0,ni=psb->m_rcontacts.size();im_rcontacts[i]; const sCti& cti = c.m_cti; if (cti.m_colObj->hasContactResponse()) { - btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); - const btVector3 vb = c.m_node->m_x-c.m_node->m_q; + btVector3 va(0,0,0); + btRigidBody* rigidCol=0; + btMultiBodyLinkCollider* multibodyLinkCol=0; + btScalar* deltaV; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + jacobianData.m_jacobians.resize(ndof); + jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); + btScalar* jac=&jacobianData.m_jacobians[0]; + + multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); + deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0],deltaV,jacobianData.scratch_r, jacobianData.scratch_v); + + btScalar vel = 0.0; + for (int j = 0; j < ndof ; ++j) { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; + } + va = cti.m_normal*vel*dt; + } + } + + const btVector3 vb = c.m_node->m_x-c.m_node->m_q; const btVector3 vr = vb-va; const btScalar dn = btDot(vr, cti.m_normal); if(dn<=SIMD_EPSILON) @@ -3041,8 +3077,20 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient const btVector3 impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst ); c.m_node->m_x -= impulse * c.m_c2; - if (tmpRigid) - tmpRigid->applyImpulse(impulse,c.m_c1); + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + if (rigidCol) + rigidCol->applyImpulse(impulse,c.m_c1); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + if (multibodyLinkCol) + { + double multiplier = 0.5; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV,-impulse.length()*multiplier); + } + } } } } @@ -3051,6 +3099,8 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) // void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti) { + BT_PROFILE("PSolve_SContacts"); + for(int i=0,ni=psb->m_scontacts.size();im_scontacts[i]; @@ -3084,6 +3134,7 @@ void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti) // void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti) { +BT_PROFILE("PSolve_Links"); for(int i=0,ni=psb->m_links.size();im_links[i]; @@ -3106,6 +3157,7 @@ void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti) // void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst) { + BT_PROFILE("VSolve_Links"); for(int i=0,ni=psb->m_links.size();im_links[i]; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.h b/extern/bullet/src/BulletSoftBody/btSoftBody.h similarity index 99% rename from extern/bullet2/src/BulletSoftBody/btSoftBody.h rename to extern/bullet/src/BulletSoftBody/btSoftBody.h index bd5846bfb67b..ada0dfd1a5cd 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.h +++ b/extern/bullet/src/BulletSoftBody/btSoftBody.h @@ -232,15 +232,18 @@ class btSoftBody : public btCollisionObject int m_battach:1; // Attached }; /* Link */ - struct Link : Feature + ATTRIBUTE_ALIGNED16(struct) Link : Feature { + btVector3 m_c3; // gradient Node* m_n[2]; // Node pointers btScalar m_rl; // Rest length int m_bbending:1; // Bending link btScalar m_c0; // (ima+imb)*kLST btScalar m_c1; // rl^2 btScalar m_c2; // |gradient|^2/c0 - btVector3 m_c3; // gradient + + BT_DECLARE_ALIGNED_ALLOCATOR(); + }; /* Face */ struct Face : Feature @@ -614,7 +617,7 @@ class btSoftBody : public btCollisionObject RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt); void Process(const btDbvtNode* leaf); - static inline btScalar rayFromToTriangle(const btVector3& rayFrom, + static /*inline*/ btScalar rayFromToTriangle(const btVector3& rayFrom, const btVector3& rayTo, const btVector3& rayNormalizedDirection, const btVector3& a, diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/extern/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp similarity index 96% rename from extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp rename to extern/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp index 9f0d44526bf4..ab84bddf2a68 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp +++ b/extern/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @@ -120,8 +120,8 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1); //btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//?? btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex); - - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr); + ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS; + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr); colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); @@ -164,7 +164,8 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1); btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);//btTransform::getIdentity());//?? - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr); + ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS; + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr); colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h b/extern/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h similarity index 100% rename from extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h rename to extern/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyData.h b/extern/bullet/src/BulletSoftBody/btSoftBodyData.h similarity index 100% rename from extern/bullet2/src/BulletSoftBody/btSoftBodyData.h rename to extern/bullet/src/BulletSoftBody/btSoftBodyData.h diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp b/extern/bullet/src/BulletSoftBody/btSoftBodyHelpers.cpp similarity index 99% rename from extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp rename to extern/bullet/src/BulletSoftBody/btSoftBodyHelpers.cpp index d96f85ec6308..51fcd16da407 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/extern/bullet/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -123,8 +123,9 @@ static inline T average(const btAlignedObjectArray& items) return(sum(items)/n); } +#if 0 // -static inline btScalar tetravolume(const btVector3& x0, + inline static btScalar tetravolume(const btVector3& x0, const btVector3& x1, const btVector3& x2, const btVector3& x3) @@ -134,6 +135,7 @@ static inline btScalar tetravolume(const btVector3& x0, const btVector3 c=x3-x0; return(btDot(a,btCross(b,c))); } +#endif // #if 0 @@ -1127,7 +1129,6 @@ int nattrb=0; int hasbounds=0; int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds); result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds); -(void)result; node += nextLine(node); pos.resize(nnode); @@ -1209,10 +1210,10 @@ if(ele&&ele[0]) } } } -printf("Nodes: %d\r\n",psb->m_nodes.size()); -printf("Links: %d\r\n",psb->m_links.size()); -printf("Faces: %d\r\n",psb->m_faces.size()); -printf("Tetras: %d\r\n",psb->m_tetras.size()); +printf("Nodes: %u\r\n",psb->m_nodes.size()); +printf("Links: %u\r\n",psb->m_links.size()); +printf("Faces: %u\r\n",psb->m_faces.size()); +printf("Tetras: %u\r\n",psb->m_tetras.size()); return(psb); } diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h b/extern/bullet/src/BulletSoftBody/btSoftBodyHelpers.h similarity index 100% rename from extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h rename to extern/bullet/src/BulletSoftBody/btSoftBodyHelpers.h diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h b/extern/bullet/src/BulletSoftBody/btSoftBodyInternals.h similarity index 99% rename from extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h rename to extern/bullet/src/BulletSoftBody/btSoftBodyInternals.h index 1b9d02d79f93..1ad82616ea0b 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h +++ b/extern/bullet/src/BulletSoftBody/btSoftBodyInternals.h @@ -422,7 +422,7 @@ static inline btVector3 BaryCoord( const btVector3& a, } // -static inline btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, +inline static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, const btVector3& a, const btVector3& b, const btScalar accuracy, @@ -451,6 +451,25 @@ static inline btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, return(-1); } +inline static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, + const btVector3& x, + btSoftBody::sMedium& medium) +{ + medium.m_velocity = btVector3(0,0,0); + medium.m_pressure = 0; + medium.m_density = wfi->air_density; + if(wfi->water_density>0) + { + const btScalar depth=-(btDot(x,wfi->water_normal)+wfi->water_offset); + if(depth>0) + { + medium.m_density = wfi->water_density; + medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length(); + } + } +} + + // static inline btVector3 NormalizeAny(const btVector3& v) { @@ -504,23 +523,7 @@ static inline btScalar VolumeOf( const btVector3& x0, } // -static inline void EvaluateMedium( const btSoftBodyWorldInfo* wfi, - const btVector3& x, - btSoftBody::sMedium& medium) -{ - medium.m_velocity = btVector3(0,0,0); - medium.m_pressure = 0; - medium.m_density = wfi->air_density; - if(wfi->water_density>0) - { - const btScalar depth=-(btDot(x,wfi->water_normal)+wfi->water_offset); - if(depth>0) - { - medium.m_density = wfi->water_density; - medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length(); - } - } -} + // static inline void ApplyClampedForce( btSoftBody::Node& n, diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp b/extern/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp similarity index 100% rename from extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp rename to extern/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h b/extern/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h similarity index 100% rename from extern/bullet2/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h rename to extern/bullet/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h b/extern/bullet/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h similarity index 100% rename from extern/bullet2/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h rename to extern/bullet/src/BulletSoftBody/btSoftBodySolverVertexBuffer.h diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h b/extern/bullet/src/BulletSoftBody/btSoftBodySolvers.h similarity index 100% rename from extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h rename to extern/bullet/src/BulletSoftBody/btSoftBodySolvers.h diff --git a/extern/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp b/extern/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp new file mode 100644 index 000000000000..6facce4e8639 --- /dev/null +++ b/extern/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp @@ -0,0 +1,371 @@ +/* +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 "btSoftMultiBodyDynamicsWorld.h" +#include "LinearMath/btQuickprof.h" + +//softbody & helpers +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btSoftBodySolvers.h" +#include "BulletSoftBody/btDefaultSoftBodySolver.h" +#include "LinearMath/btSerializer.h" + + +btSoftMultiBodyDynamicsWorld::btSoftMultiBodyDynamicsWorld( + btDispatcher* dispatcher, + btBroadphaseInterface* pairCache, + btMultiBodyConstraintSolver* constraintSolver, + btCollisionConfiguration* collisionConfiguration, + btSoftBodySolver *softBodySolver ) : + btMultiBodyDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), + m_softBodySolver( softBodySolver ), + m_ownsSolver(false) +{ + if( !m_softBodySolver ) + { + void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16); + m_softBodySolver = new(ptr) btDefaultSoftBodySolver(); + m_ownsSolver = true; + } + + m_drawFlags = fDrawFlags::Std; + m_drawNodeTree = true; + m_drawFaceTree = false; + m_drawClusterTree = false; + m_sbi.m_broadphase = pairCache; + m_sbi.m_dispatcher = dispatcher; + m_sbi.m_sparsesdf.Initialize(); + m_sbi.m_sparsesdf.Reset(); + + m_sbi.air_density = (btScalar)1.2; + m_sbi.water_density = 0; + m_sbi.water_offset = 0; + m_sbi.water_normal = btVector3(0,0,0); + m_sbi.m_gravity.setValue(0,-10,0); + + m_sbi.m_sparsesdf.Initialize(); + + +} + +btSoftMultiBodyDynamicsWorld::~btSoftMultiBodyDynamicsWorld() +{ + if (m_ownsSolver) + { + m_softBodySolver->~btSoftBodySolver(); + btAlignedFree(m_softBodySolver); + } +} + +void btSoftMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep ); + { + BT_PROFILE("predictUnconstraintMotionSoftBody"); + m_softBodySolver->predictMotion( float(timeStep) ); + } +} + +void btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation( btScalar timeStep ) +{ + + // Let the solver grab the soft bodies and if necessary optimize for it + m_softBodySolver->optimize( getSoftBodyArray() ); + + if( !m_softBodySolver->checkInitialized() ) + { + btAssert( "Solver initialization failed\n" ); + } + + btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); + + ///solve soft bodies constraints + solveSoftBodiesConstraints( timeStep ); + + //self collisions + for ( int i=0;idefaultCollisionHandler(psb); + } + + ///update soft bodies + m_softBodySolver->updateSoftBodies( ); + + // End solver-wise simulation step + // /////////////////////////////// + +} + +void btSoftMultiBodyDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep ) +{ + BT_PROFILE("solveSoftConstraints"); + + if(m_softBodies.size()) + { + btSoftBody::solveClusters(m_softBodies); + } + + // Solve constraints solver-wise + m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() ); + +} + +void btSoftMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) +{ + m_softBodies.push_back(body); + + // Set the soft body solver that will deal with this body + // to be the world's solver + body->setSoftBodySolver( m_softBodySolver ); + + btCollisionWorld::addCollisionObject(body, + collisionFilterGroup, + collisionFilterMask); + +} + +void btSoftMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body) +{ + m_softBodies.remove(body); + + btCollisionWorld::removeCollisionObject(body); +} + +void btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +{ + btSoftBody* body = btSoftBody::upcast(collisionObject); + if (body) + removeSoftBody(body); + else + btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); +} + +void btSoftMultiBodyDynamicsWorld::debugDrawWorld() +{ + btMultiBodyDynamicsWorld::debugDrawWorld(); + + if (getDebugDrawer()) + { + int i; + for ( i=0;im_softBodies.size();i++) + { + btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; + if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); + btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags); + } + + if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + if(m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); + if(m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer); + if(m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer); + } + } + } +} + + + + +struct btSoftSingleRayCallback : public btBroadphaseRayCallback +{ + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; + + const btSoftMultiBodyDynamicsWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; + + btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftMultiBodyDynamicsWorld* 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/1e30 + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + m_signs[0] = m_rayDirectionInverse[0] < 0.0; + m_signs[1] = m_rayDirectionInverse[1] < 0.0; + m_signs[2] = m_rayDirectionInverse[2] < 0.0; + + m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld); + + } + + + + virtual bool process(const btBroadphaseProxy* proxy) + { + ///terminate further ray tests, once the closestHitFraction reached zero + if (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 btSoftMultiBodyDynamicsWorld::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 + btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); + +#ifndef USE_BRUTEFORCE_RAYBROADPHASE + m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); +#else + for (int i=0;igetNumCollisionObjects();i++) + { + rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE + +} + + +void btSoftMultiBodyDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) +{ + if (collisionShape->isSoftBody()) { + btSoftBody* softBody = btSoftBody::upcast(collisionObject); + if (softBody) { + btSoftBody::sRayCast softResult; + if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) + { + + if (softResult.fraction<= resultCallback.m_closestHitFraction) + { + + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = 0; + shapeInfo.m_triangleIndex = softResult.index; + // get the normal + btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); + btVector3 normal=-rayDir; + normal.normalize(); + + if (softResult.feature == btSoftBody::eFeature::Face) + { + normal = softBody->m_faces[softResult.index].m_normal; + if (normal.dot(rayDir) > 0) { + // normal always point toward origin of the ray + normal = -normal; + } + } + + btCollisionWorld::LocalRayResult rayResult + (collisionObject, + &shapeInfo, + normal, + softResult.fraction); + bool normalInWorldSpace = true; + resultCallback.addSingleResult(rayResult,normalInWorldSpace); + } + } + } + } + else { + btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback); + } +} + + +void btSoftMultiBodyDynamicsWorld::serializeSoftBodies(btSerializer* serializer) +{ + int i; + //serialize all collision objects + for (i=0;igetInternalType() & btCollisionObject::CO_SOFT_BODY) + { + int len = colObj->calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(len,1); + const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj); + } + } + +} + +void btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer) +{ + + serializer->startSerialization(); + + serializeDynamicsWorldInfo( serializer); + + serializeSoftBodies(serializer); + + serializeMultiBodies(serializer); + + serializeRigidBodies(serializer); + + serializeCollisionObjects(serializer); + + serializeContactManifolds(serializer); + + serializer->finishSerialization(); +} + + diff --git a/extern/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h b/extern/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h new file mode 100644 index 000000000000..6d46a21db5c0 --- /dev/null +++ b/extern/bullet/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h @@ -0,0 +1,110 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H +#define BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H + +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" + +#ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_H +typedef btAlignedObjectArray btSoftBodyArray; +#endif + +class btSoftBodySolver; + +class btSoftMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld +{ + + btSoftBodyArray m_softBodies; + int m_drawFlags; + bool m_drawNodeTree; + bool m_drawFaceTree; + bool m_drawClusterTree; + btSoftBodyWorldInfo m_sbi; + ///Solver classes that encapsulate multiple soft bodies for solving + btSoftBodySolver *m_softBodySolver; + bool m_ownsSolver; + +protected: + + virtual void predictUnconstraintMotion(btScalar timeStep); + + virtual void internalSingleStepSimulation( btScalar timeStep); + + void solveSoftBodiesConstraints( btScalar timeStep ); + + void serializeSoftBodies(btSerializer* serializer); + +public: + + btSoftMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 ); + + virtual ~btSoftMultiBodyDynamicsWorld(); + + virtual void debugDrawWorld(); + + void addSoftBody(btSoftBody* body, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter); + + void removeSoftBody(btSoftBody* body); + + ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject + virtual void removeCollisionObject(btCollisionObject* collisionObject); + + int getDrawFlags() const { return(m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags=f; } + + btSoftBodyWorldInfo& getWorldInfo() + { + return m_sbi; + } + const btSoftBodyWorldInfo& getWorldInfo() const + { + return m_sbi; + } + + virtual btDynamicsWorldType getWorldType() const + { + return BT_SOFT_MULTIBODY_DYNAMICS_WORLD; + } + + btSoftBodyArray& getSoftBodyArray() + { + return m_softBodies; + } + + const btSoftBodyArray& getSoftBodyArray() const + { + return m_softBodies; + } + + + virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; + + /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. + /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. + /// This allows more customization. + static void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback); + + virtual void serialize(btSerializer* serializer); + +}; + +#endif //BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/extern/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp similarity index 100% rename from extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp rename to extern/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h b/extern/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h similarity index 97% rename from extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h rename to extern/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h index a9b513e36397..93fcc6065b5e 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h +++ b/extern/bullet/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h @@ -31,8 +31,8 @@ class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm // bool m_ownManifold; // btPersistentManifold* m_manifoldPtr; - btSoftBody* m_softBody; - btCollisionObject* m_rigidCollisionObject; + //btSoftBody* m_softBody; + //btCollisionObject* m_rigidCollisionObject; ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean bool m_isSwapped; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/extern/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp similarity index 98% rename from extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp rename to extern/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index 653d5a06b496..204b4f576d57 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/extern/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -125,7 +125,7 @@ void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep ) } -void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask) +void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) { m_softBodies.push_back(body); diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h b/extern/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.h similarity index 95% rename from extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h rename to extern/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.h index 3e0efafd6c76..d921a6488d33 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h +++ b/extern/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorld.h @@ -54,7 +54,7 @@ class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld virtual void debugDrawWorld(); - void addSoftBody(btSoftBody* body,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter); + void addSoftBody(btSoftBody* body, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter); void removeSoftBody(btSoftBody* body); diff --git a/extern/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorldMt.cpp b/extern/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorldMt.cpp new file mode 100644 index 000000000000..05301439ad01 --- /dev/null +++ b/extern/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorldMt.cpp @@ -0,0 +1,368 @@ +/* +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 "btSoftRigidDynamicsWorldMt.h" +#include "LinearMath/btQuickprof.h" + +//softbody & helpers +#include "btSoftBody.h" +#include "btSoftBodyHelpers.h" +#include "btSoftBodySolvers.h" +#include "btDefaultSoftBodySolver.h" +#include "LinearMath/btSerializer.h" + + +btSoftRigidDynamicsWorldMt::btSoftRigidDynamicsWorldMt( + btDispatcher* dispatcher, + btBroadphaseInterface* pairCache, + btConstraintSolverPoolMt* constraintSolver, + btConstraintSolver* constraintSolverMt, + btCollisionConfiguration* collisionConfiguration, + btSoftBodySolver *softBodySolver ) : + btDiscreteDynamicsWorldMt(dispatcher,pairCache,constraintSolver,constraintSolverMt,collisionConfiguration), + m_softBodySolver( softBodySolver ), + m_ownsSolver(false) +{ + if( !m_softBodySolver ) + { + void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16); + m_softBodySolver = new(ptr) btDefaultSoftBodySolver(); + m_ownsSolver = true; + } + + m_drawFlags = fDrawFlags::Std; + m_drawNodeTree = true; + m_drawFaceTree = false; + m_drawClusterTree = false; + m_sbi.m_broadphase = pairCache; + m_sbi.m_dispatcher = dispatcher; + m_sbi.m_sparsesdf.Initialize(); + m_sbi.m_sparsesdf.Reset(); + + m_sbi.air_density = (btScalar)1.2; + m_sbi.water_density = 0; + m_sbi.water_offset = 0; + m_sbi.water_normal = btVector3(0,0,0); + m_sbi.m_gravity.setValue(0,-10,0); + + m_sbi.m_sparsesdf.Initialize(); + + +} + +btSoftRigidDynamicsWorldMt::~btSoftRigidDynamicsWorldMt() +{ + if (m_ownsSolver) + { + m_softBodySolver->~btSoftBodySolver(); + btAlignedFree(m_softBodySolver); + } +} + +void btSoftRigidDynamicsWorldMt::predictUnconstraintMotion(btScalar timeStep) +{ + btDiscreteDynamicsWorldMt::predictUnconstraintMotion( timeStep ); + { + BT_PROFILE("predictUnconstraintMotionSoftBody"); + m_softBodySolver->predictMotion( float(timeStep) ); + } +} + +void btSoftRigidDynamicsWorldMt::internalSingleStepSimulation( btScalar timeStep ) +{ + + // Let the solver grab the soft bodies and if necessary optimize for it + m_softBodySolver->optimize( getSoftBodyArray() ); + + if( !m_softBodySolver->checkInitialized() ) + { + btAssert( "Solver initialization failed\n" ); + } + + btDiscreteDynamicsWorldMt::internalSingleStepSimulation( timeStep ); + + ///solve soft bodies constraints + solveSoftBodiesConstraints( timeStep ); + + //self collisions + for ( int i=0;idefaultCollisionHandler(psb); + } + + ///update soft bodies + m_softBodySolver->updateSoftBodies( ); + + // End solver-wise simulation step + // /////////////////////////////// + +} + +void btSoftRigidDynamicsWorldMt::solveSoftBodiesConstraints( btScalar timeStep ) +{ + BT_PROFILE("solveSoftConstraints"); + + if(m_softBodies.size()) + { + btSoftBody::solveClusters(m_softBodies); + } + + // Solve constraints solver-wise + m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() ); + +} + +void btSoftRigidDynamicsWorldMt::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) +{ + m_softBodies.push_back(body); + + // Set the soft body solver that will deal with this body + // to be the world's solver + body->setSoftBodySolver( m_softBodySolver ); + + btCollisionWorld::addCollisionObject(body, + collisionFilterGroup, + collisionFilterMask); + +} + +void btSoftRigidDynamicsWorldMt::removeSoftBody(btSoftBody* body) +{ + m_softBodies.remove(body); + + btCollisionWorld::removeCollisionObject(body); +} + +void btSoftRigidDynamicsWorldMt::removeCollisionObject(btCollisionObject* collisionObject) +{ + btSoftBody* body = btSoftBody::upcast(collisionObject); + if (body) + removeSoftBody(body); + else + btDiscreteDynamicsWorldMt::removeCollisionObject(collisionObject); +} + +void btSoftRigidDynamicsWorldMt::debugDrawWorld() +{ + btDiscreteDynamicsWorldMt::debugDrawWorld(); + + if (getDebugDrawer()) + { + int i; + for ( i=0;im_softBodies.size();i++) + { + btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; + if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); + btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags); + } + + if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + if(m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); + if(m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer); + if(m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer); + } + } + } +} + + + + +struct btSoftSingleRayCallback : public btBroadphaseRayCallback +{ + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; + + const btSoftRigidDynamicsWorldMt* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; + + btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftRigidDynamicsWorldMt* 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/1e30 + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + m_signs[0] = m_rayDirectionInverse[0] < 0.0; + m_signs[1] = m_rayDirectionInverse[1] < 0.0; + m_signs[2] = m_rayDirectionInverse[2] < 0.0; + + m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld); + + } + + + + virtual bool process(const btBroadphaseProxy* proxy) + { + ///terminate further ray tests, once the closestHitFraction reached zero + if (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 btSoftRigidDynamicsWorldMt::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 + btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); + +#ifndef USE_BRUTEFORCE_RAYBROADPHASE + m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); +#else + for (int i=0;igetNumCollisionObjects();i++) + { + rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE + +} + + +void btSoftRigidDynamicsWorldMt::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) +{ + if (collisionShape->isSoftBody()) { + btSoftBody* softBody = btSoftBody::upcast(collisionObject); + if (softBody) { + btSoftBody::sRayCast softResult; + if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) + { + + if (softResult.fraction<= resultCallback.m_closestHitFraction) + { + + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = 0; + shapeInfo.m_triangleIndex = softResult.index; + // get the normal + btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); + btVector3 normal=-rayDir; + normal.normalize(); + + if (softResult.feature == btSoftBody::eFeature::Face) + { + normal = softBody->m_faces[softResult.index].m_normal; + if (normal.dot(rayDir) > 0) { + // normal always point toward origin of the ray + normal = -normal; + } + } + + btCollisionWorld::LocalRayResult rayResult + (collisionObject, + &shapeInfo, + normal, + softResult.fraction); + bool normalInWorldSpace = true; + resultCallback.addSingleResult(rayResult,normalInWorldSpace); + } + } + } + } + else { + btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback); + } +} + + +void btSoftRigidDynamicsWorldMt::serializeSoftBodies(btSerializer* serializer) +{ + int i; + //serialize all collision objects + for (i=0;igetInternalType() & btCollisionObject::CO_SOFT_BODY) + { + int len = colObj->calculateSerializeBufferSize(); + btChunk* chunk = serializer->allocate(len,1); + const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj); + } + } + +} + +void btSoftRigidDynamicsWorldMt::serialize(btSerializer* serializer) +{ + + serializer->startSerialization(); + + serializeDynamicsWorldInfo( serializer); + + serializeSoftBodies(serializer); + + serializeRigidBodies(serializer); + + serializeCollisionObjects(serializer); + + serializer->finishSerialization(); +} + + diff --git a/extern/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorldMt.h b/extern/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorldMt.h new file mode 100644 index 000000000000..516fd3fe422d --- /dev/null +++ b/extern/bullet/src/BulletSoftBody/btSoftRigidDynamicsWorldMt.h @@ -0,0 +1,107 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_MT_H +#define BT_SOFT_RIGID_DYNAMICS_WORLD_MT_H + +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h" +#include "btSoftBody.h" + +typedef btAlignedObjectArray btSoftBodyArray; + +class btSoftBodySolver; + +class btSoftRigidDynamicsWorldMt : public btDiscreteDynamicsWorldMt +{ + + btSoftBodyArray m_softBodies; + int m_drawFlags; + bool m_drawNodeTree; + bool m_drawFaceTree; + bool m_drawClusterTree; + btSoftBodyWorldInfo m_sbi; + ///Solver classes that encapsulate multiple soft bodies for solving + btSoftBodySolver *m_softBodySolver; + bool m_ownsSolver; + +protected: + + virtual void predictUnconstraintMotion(btScalar timeStep); + + virtual void internalSingleStepSimulation( btScalar timeStep); + + void solveSoftBodiesConstraints( btScalar timeStep ); + + void serializeSoftBodies(btSerializer* serializer); + +public: + + btSoftRigidDynamicsWorldMt(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolverPoolMt* constraintSolver, btConstraintSolver* constraintSolverMt, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 ); + + virtual ~btSoftRigidDynamicsWorldMt(); + + virtual void debugDrawWorld(); + + void addSoftBody(btSoftBody* body, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter); + + void removeSoftBody(btSoftBody* body); + + ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorldMt::removeCollisionObject + virtual void removeCollisionObject(btCollisionObject* collisionObject); + + int getDrawFlags() const { return(m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags=f; } + + btSoftBodyWorldInfo& getWorldInfo() + { + return m_sbi; + } + const btSoftBodyWorldInfo& getWorldInfo() const + { + return m_sbi; + } + + virtual btDynamicsWorldType getWorldType() const + { + return BT_SOFT_RIGID_DYNAMICS_WORLD; + } + + btSoftBodyArray& getSoftBodyArray() + { + return m_softBodies; + } + + const btSoftBodyArray& getSoftBodyArray() const + { + return m_softBodies; + } + + + virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; + + /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. + /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. + /// This allows more customization. + static void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback); + + virtual void serialize(btSerializer* serializer); + +}; + +#endif //BT_SOFT_RIGID_DYNAMICS_WORLD_MT_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp b/extern/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp similarity index 100% rename from extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp rename to extern/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp diff --git a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h b/extern/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h similarity index 98% rename from extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h rename to extern/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h index 43b1439cc536..4eab7aea2fef 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h +++ b/extern/bullet/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h @@ -30,8 +30,8 @@ class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm bool m_ownManifold; btPersistentManifold* m_manifoldPtr; - btSoftBody* m_softBody0; - btSoftBody* m_softBody1; +// btSoftBody* m_softBody0; +// btSoftBody* m_softBody1; public: diff --git a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h b/extern/bullet/src/BulletSoftBody/btSparseSDF.h similarity index 99% rename from extern/bullet2/src/BulletSoftBody/btSparseSDF.h rename to extern/bullet/src/BulletSoftBody/btSparseSDF.h index 8992ddbb68d1..ba437c28efc2 100644 --- a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h +++ b/extern/bullet/src/BulletSoftBody/btSparseSDF.h @@ -185,6 +185,7 @@ struct btSparseSdf { ++nprobes; ++ncells; + //int sz = sizeof(Cell); if (ncells>m_clampCells) { static int numResets=0; diff --git a/extern/bullet/src/BulletSoftBody/premake4.lua b/extern/bullet/src/BulletSoftBody/premake4.lua new file mode 100644 index 000000000000..a75e336194d9 --- /dev/null +++ b/extern/bullet/src/BulletSoftBody/premake4.lua @@ -0,0 +1,14 @@ + project "BulletSoftBody" + + kind "StaticLib" + + includedirs { + "..", + } + if os.is("Linux") then + buildoptions{"-fPIC"} + end + files { + "**.cpp", + "**.h" + } \ No newline at end of file diff --git a/extern/bullet/src/CMakeLists.txt b/extern/bullet/src/CMakeLists.txt new file mode 100644 index 000000000000..c30125c53902 --- /dev/null +++ b/extern/bullet/src/CMakeLists.txt @@ -0,0 +1,19 @@ + +IF(BUILD_BULLET3) + SUBDIRS( Bullet3OpenCL Bullet3Serialize/Bullet2FileLoader Bullet3Dynamics Bullet3Collision Bullet3Geometry ) +ENDIF(BUILD_BULLET3) + + +SUBDIRS( BulletInverseDynamics BulletSoftBody BulletCollision BulletDynamics LinearMath Bullet3Common) + + +IF(INSTALL_LIBS) + #INSTALL of other files requires CMake 2.6 + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + # Don't actually need to install any common files, the frameworks include everything + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(FILES btBulletCollisionCommon.h btBulletDynamicsCommon.h DESTINATION ${INCLUDE_INSTALL_DIR}) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) +ENDIF(INSTALL_LIBS) diff --git a/extern/bullet/src/LinearMath/CMakeLists.txt b/extern/bullet/src/LinearMath/CMakeLists.txt new file mode 100644 index 000000000000..0c8c0133a325 --- /dev/null +++ b/extern/bullet/src/LinearMath/CMakeLists.txt @@ -0,0 +1,79 @@ + +INCLUDE_DIRECTORIES( + ${BULLET_PHYSICS_SOURCE_DIR}/src +) + +SET(LinearMath_SRCS + btAlignedAllocator.cpp + btConvexHull.cpp + btConvexHullComputer.cpp + btGeometryUtil.cpp + btPolarDecomposition.cpp + btQuickprof.cpp + btSerializer.cpp + btSerializer64.cpp + btThreads.cpp + btVector3.cpp + TaskScheduler/btTaskScheduler.cpp + TaskScheduler/btThreadSupportPosix.cpp + TaskScheduler/btThreadSupportWin32.cpp +) + +SET(LinearMath_HDRS + btAabbUtil2.h + btAlignedAllocator.h + btAlignedObjectArray.h + btConvexHull.h + btConvexHullComputer.h + btDefaultMotionState.h + btGeometryUtil.h + btGrahamScan2dConvexHull.h + btHashMap.h + btIDebugDraw.h + btList.h + btMatrix3x3.h + btMinMax.h + btMotionState.h + btPolarDecomposition.h + btPoolAllocator.h + btQuadWord.h + btQuaternion.h + btQuickprof.h + btRandom.h + btScalar.h + btSerializer.h + btStackAlloc.h + btThreads.h + btTransform.h + btTransformUtil.h + btVector3.h + TaskScheduler/btThreadSupportInterface.h +) + +ADD_LIBRARY(LinearMath ${LinearMath_SRCS} ${LinearMath_HDRS}) +SET_TARGET_PROPERTIES(LinearMath PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(LinearMath PROPERTIES SOVERSION ${BULLET_VERSION}) + +IF (INSTALL_LIBS) + IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) + #FILES_MATCHING requires CMake 2.6 + IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS LinearMath DESTINATION .) + ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + INSTALL(TARGETS LinearMath + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib${LIB_SUFFIX} + ARCHIVE DESTINATION lib${LIB_SUFFIX}) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN +".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE) + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + + IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(LinearMath PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(LinearMath PROPERTIES PUBLIC_HEADER "${LinearMath_HDRS}") + ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) +ENDIF (INSTALL_LIBS) diff --git a/extern/bullet/src/LinearMath/TaskScheduler/btTaskScheduler.cpp b/extern/bullet/src/LinearMath/TaskScheduler/btTaskScheduler.cpp new file mode 100644 index 000000000000..49510d16601b --- /dev/null +++ b/extern/bullet/src/LinearMath/TaskScheduler/btTaskScheduler.cpp @@ -0,0 +1,802 @@ + +#include "LinearMath/btMinMax.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btThreads.h" +#include "LinearMath/btQuickprof.h" +#include +#include + + + +#if BT_THREADSAFE + +#include "btThreadSupportInterface.h" + +#if defined( _WIN32 ) + +#define WIN32_LEAN_AND_MEAN + +#include + +#endif + + +typedef unsigned long long btU64; +static const int kCacheLineSize = 64; + +void btSpinPause() +{ +#if defined( _WIN32 ) + YieldProcessor(); +#endif +} + + +struct WorkerThreadStatus +{ + enum Type + { + kInvalid, + kWaitingForWork, + kWorking, + kSleeping, + }; +}; + + +ATTRIBUTE_ALIGNED64(class) WorkerThreadDirectives +{ + static const int kMaxThreadCount = BT_MAX_THREAD_COUNT; + // directives for all worker threads packed into a single cacheline + char m_threadDirs[kMaxThreadCount]; + +public: + enum Type + { + kInvalid, + kGoToSleep, // go to sleep + kStayAwakeButIdle, // wait for not checking job queue + kScanForJobs, // actively scan job queue for jobs + }; + WorkerThreadDirectives() + { + for ( int i = 0; i < kMaxThreadCount; ++i ) + { + m_threadDirs[ i ] = 0; + } + } + + Type getDirective(int threadId) + { + btAssert(threadId < kMaxThreadCount); + return static_cast(m_threadDirs[threadId]); + } + + void setDirectiveByRange(int threadBegin, int threadEnd, Type dir) + { + btAssert( threadBegin < threadEnd ); + btAssert( threadEnd <= kMaxThreadCount ); + char dirChar = static_cast(dir); + for ( int i = threadBegin; i < threadEnd; ++i ) + { + m_threadDirs[ i ] = dirChar; + } + } +}; + +class JobQueue; + +ATTRIBUTE_ALIGNED64(struct) ThreadLocalStorage +{ + int m_threadId; + WorkerThreadStatus::Type m_status; + int m_numJobsFinished; + btSpinMutex m_mutex; + btScalar m_sumResult; + WorkerThreadDirectives * m_directive; + JobQueue* m_queue; + btClock* m_clock; + unsigned int m_cooldownTime; +}; + + +struct IJob +{ + virtual void executeJob(int threadId) = 0; +}; + +class ParallelForJob : public IJob +{ + const btIParallelForBody* m_body; + int m_begin; + int m_end; + +public: + ParallelForJob( int iBegin, int iEnd, const btIParallelForBody& body ) + { + m_body = &body; + m_begin = iBegin; + m_end = iEnd; + } + virtual void executeJob(int threadId) BT_OVERRIDE + { + BT_PROFILE( "executeJob" ); + + // call the functor body to do the work + m_body->forLoop( m_begin, m_end ); + } +}; + + +class ParallelSumJob : public IJob +{ + const btIParallelSumBody* m_body; + ThreadLocalStorage* m_threadLocalStoreArray; + int m_begin; + int m_end; + +public: + ParallelSumJob( int iBegin, int iEnd, const btIParallelSumBody& body, ThreadLocalStorage* tls ) + { + m_body = &body; + m_threadLocalStoreArray = tls; + m_begin = iBegin; + m_end = iEnd; + } + virtual void executeJob( int threadId ) BT_OVERRIDE + { + BT_PROFILE( "executeJob" ); + + // call the functor body to do the work + btScalar val = m_body->sumLoop( m_begin, m_end ); +#if BT_PARALLEL_SUM_DETERMINISTISM + // by truncating bits of the result, we can make the parallelSum deterministic (at the expense of precision) + const float TRUNC_SCALE = float(1<<19); + val = floor(val*TRUNC_SCALE+0.5f)/TRUNC_SCALE; // truncate some bits +#endif + m_threadLocalStoreArray[threadId].m_sumResult += val; + } +}; + + +ATTRIBUTE_ALIGNED64(class) JobQueue +{ + btThreadSupportInterface* m_threadSupport; + btCriticalSection* m_queueLock; + btSpinMutex m_mutex; + + btAlignedObjectArray m_jobQueue; + char* m_jobMem; + int m_jobMemSize; + bool m_queueIsEmpty; + int m_tailIndex; + int m_headIndex; + int m_allocSize; + bool m_useSpinMutex; + btAlignedObjectArray m_neighborContexts; + char m_cachePadding[kCacheLineSize]; // prevent false sharing + + void freeJobMem() + { + if ( m_jobMem ) + { + // free old + btAlignedFree(m_jobMem); + m_jobMem = NULL; + } + } + void resizeJobMem(int newSize) + { + if (newSize > m_jobMemSize) + { + freeJobMem(); + m_jobMem = static_cast(btAlignedAlloc(newSize, kCacheLineSize)); + m_jobMemSize = newSize; + } + } + +public: + + JobQueue() + { + m_jobMem = NULL; + m_jobMemSize = 0; + m_threadSupport = NULL; + m_queueLock = NULL; + m_headIndex = 0; + m_tailIndex = 0; + m_useSpinMutex = false; + } + ~JobQueue() + { + exit(); + } + void exit() + { + freeJobMem(); + if (m_queueLock && m_threadSupport) + { + m_threadSupport->deleteCriticalSection(m_queueLock); + m_queueLock = NULL; + m_threadSupport = 0; + } + } + + void init(btThreadSupportInterface* threadSup, btAlignedObjectArray* contextArray) + { + m_threadSupport = threadSup; + if (threadSup) + { + m_queueLock = m_threadSupport->createCriticalSection(); + } + setupJobStealing(contextArray, contextArray->size()); + } + void setupJobStealing(btAlignedObjectArray* contextArray, int numActiveContexts) + { + btAlignedObjectArray& contexts = *contextArray; + int selfIndex = 0; + for (int i = 0; i < contexts.size(); ++i) + { + if ( this == &contexts[ i ] ) + { + selfIndex = i; + break; + } + } + int numNeighbors = btMin(2, contexts.size() - 1); + int neighborOffsets[ ] = {-1, 1, -2, 2, -3, 3}; + int numOffsets = sizeof(neighborOffsets)/sizeof(neighborOffsets[0]); + m_neighborContexts.reserve( numNeighbors ); + m_neighborContexts.resizeNoInitialize(0); + for (int i = 0; i < numOffsets && m_neighborContexts.size() < numNeighbors; i++) + { + int neighborIndex = selfIndex + neighborOffsets[i]; + if ( neighborIndex >= 0 && neighborIndex < numActiveContexts) + { + m_neighborContexts.push_back( &contexts[ neighborIndex ] ); + } + } + } + + bool isQueueEmpty() const {return m_queueIsEmpty;} + void lockQueue() + { + if ( m_useSpinMutex ) + { + m_mutex.lock(); + } + else + { + m_queueLock->lock(); + } + } + void unlockQueue() + { + if ( m_useSpinMutex ) + { + m_mutex.unlock(); + } + else + { + m_queueLock->unlock(); + } + } + void clearQueue(int jobCount, int jobSize) + { + lockQueue(); + m_headIndex = 0; + m_tailIndex = 0; + m_allocSize = 0; + m_queueIsEmpty = true; + int jobBufSize = jobSize * jobCount; + // make sure we have enough memory allocated to store jobs + if ( jobBufSize > m_jobMemSize ) + { + resizeJobMem( jobBufSize ); + } + // make sure job queue is big enough + if ( jobCount > m_jobQueue.capacity() ) + { + m_jobQueue.reserve( jobCount ); + } + unlockQueue(); + m_jobQueue.resizeNoInitialize( 0 ); + } + void* allocJobMem(int jobSize) + { + btAssert(m_jobMemSize >= (m_allocSize + jobSize)); + void* jobMem = &m_jobMem[m_allocSize]; + m_allocSize += jobSize; + return jobMem; + } + void submitJob( IJob* job ) + { + btAssert( reinterpret_cast( job ) >= &m_jobMem[ 0 ] && reinterpret_cast( job ) < &m_jobMem[ 0 ] + m_allocSize ); + m_jobQueue.push_back( job ); + lockQueue(); + m_tailIndex++; + m_queueIsEmpty = false; + unlockQueue(); + } + IJob* consumeJobFromOwnQueue() + { + if ( m_queueIsEmpty ) + { + // lock free path. even if this is taken erroneously it isn't harmful + return NULL; + } + IJob* job = NULL; + lockQueue(); + if ( !m_queueIsEmpty ) + { + job = m_jobQueue[ m_headIndex++ ]; + btAssert( reinterpret_cast( job ) >= &m_jobMem[ 0 ] && reinterpret_cast( job ) < &m_jobMem[ 0 ] + m_allocSize ); + if ( m_headIndex == m_tailIndex ) + { + m_queueIsEmpty = true; + } + } + unlockQueue(); + return job; + } + IJob* consumeJob() + { + if (IJob* job = consumeJobFromOwnQueue()) + { + return job; + } + // own queue is empty, try to steal from neighbor + for (int i = 0; i < m_neighborContexts.size(); ++i) + { + JobQueue* otherContext = m_neighborContexts[ i ]; + if ( IJob* job = otherContext->consumeJobFromOwnQueue() ) + { + return job; + } + } + return NULL; + } +}; + + +static void WorkerThreadFunc( void* userPtr ) +{ + BT_PROFILE( "WorkerThreadFunc" ); + ThreadLocalStorage* localStorage = (ThreadLocalStorage*) userPtr; + JobQueue* jobQueue = localStorage->m_queue; + + bool shouldSleep = false; + int threadId = localStorage->m_threadId; + while (! shouldSleep) + { + // do work + localStorage->m_mutex.lock(); + while ( IJob* job = jobQueue->consumeJob() ) + { + localStorage->m_status = WorkerThreadStatus::kWorking; + job->executeJob( threadId ); + localStorage->m_numJobsFinished++; + } + localStorage->m_status = WorkerThreadStatus::kWaitingForWork; + localStorage->m_mutex.unlock(); + btU64 clockStart = localStorage->m_clock->getTimeMicroseconds(); + // while queue is empty, + while (jobQueue->isQueueEmpty()) + { + // todo: spin wait a bit to avoid hammering the empty queue + btSpinPause(); + if ( localStorage->m_directive->getDirective(threadId) == WorkerThreadDirectives::kGoToSleep ) + { + shouldSleep = true; + break; + } + // if jobs are incoming, + if ( localStorage->m_directive->getDirective( threadId ) == WorkerThreadDirectives::kScanForJobs ) + { + clockStart = localStorage->m_clock->getTimeMicroseconds(); // reset clock + } + else + { + for ( int i = 0; i < 50; ++i ) + { + btSpinPause(); + btSpinPause(); + btSpinPause(); + btSpinPause(); + if (localStorage->m_directive->getDirective( threadId ) == WorkerThreadDirectives::kScanForJobs || !jobQueue->isQueueEmpty()) + { + break; + } + } + // if no jobs incoming and queue has been empty for the cooldown time, sleep + btU64 timeElapsed = localStorage->m_clock->getTimeMicroseconds() - clockStart; + if (timeElapsed > localStorage->m_cooldownTime) + { + shouldSleep = true; + break; + } + } + } + } + { + BT_PROFILE("sleep"); + // go sleep + localStorage->m_mutex.lock(); + localStorage->m_status = WorkerThreadStatus::kSleeping; + localStorage->m_mutex.unlock(); + } +} + + +class btTaskSchedulerDefault : public btITaskScheduler +{ + btThreadSupportInterface* m_threadSupport; + WorkerThreadDirectives* m_workerDirective; + btAlignedObjectArray m_jobQueues; + btAlignedObjectArray m_perThreadJobQueues; + btAlignedObjectArray m_threadLocalStorage; + btSpinMutex m_antiNestingLock; // prevent nested parallel-for + btClock m_clock; + int m_numThreads; + int m_numWorkerThreads; + int m_numActiveJobQueues; + int m_maxNumThreads; + int m_numJobs; + static const int kFirstWorkerThreadId = 1; +public: + + btTaskSchedulerDefault() : btITaskScheduler("ThreadSupport") + { + m_threadSupport = NULL; + m_workerDirective = NULL; + } + + virtual ~btTaskSchedulerDefault() + { + waitForWorkersToSleep(); + + for ( int i = 0; i < m_jobQueues.size(); ++i ) + { + m_jobQueues[i].exit(); + } + + if (m_threadSupport) + { + delete m_threadSupport; + m_threadSupport = NULL; + } + if (m_workerDirective) + { + btAlignedFree(m_workerDirective); + m_workerDirective = NULL; + } + } + + void init() + { + btThreadSupportInterface::ConstructionInfo constructionInfo( "TaskScheduler", WorkerThreadFunc ); + m_threadSupport = btThreadSupportInterface::create( constructionInfo ); + m_workerDirective = static_cast(btAlignedAlloc(sizeof(*m_workerDirective), 64)); + + m_numWorkerThreads = m_threadSupport->getNumWorkerThreads(); + m_maxNumThreads = m_threadSupport->getNumWorkerThreads() + 1; + m_numThreads = m_maxNumThreads; + // ideal to have one job queue for each physical processor (except for the main thread which needs no queue) + int numThreadsPerQueue = m_threadSupport->getLogicalToPhysicalCoreRatio(); + int numJobQueues = (numThreadsPerQueue == 1) ? (m_maxNumThreads-1) : (m_maxNumThreads / numThreadsPerQueue); + m_jobQueues.resize(numJobQueues); + m_numActiveJobQueues = numJobQueues; + for ( int i = 0; i < m_jobQueues.size(); ++i ) + { + m_jobQueues[i].init( m_threadSupport, &m_jobQueues ); + } + m_perThreadJobQueues.resize(m_numThreads); + for ( int i = 0; i < m_numThreads; i++ ) + { + JobQueue* jq = NULL; + // only worker threads get a job queue + if (i > 0) + { + if (numThreadsPerQueue == 1) + { + // one queue per worker thread + jq = &m_jobQueues[ i - kFirstWorkerThreadId ]; + } + else + { + // 2 threads share each queue + jq = &m_jobQueues[ i / numThreadsPerQueue ]; + } + } + m_perThreadJobQueues[i] = jq; + } + m_threadLocalStorage.resize(m_numThreads); + for ( int i = 0; i < m_numThreads; i++ ) + { + ThreadLocalStorage& storage = m_threadLocalStorage[i]; + storage.m_threadId = i; + storage.m_directive = m_workerDirective; + storage.m_status = WorkerThreadStatus::kSleeping; + storage.m_cooldownTime = 100; // 100 microseconds, threads go to sleep after this long if they have nothing to do + storage.m_clock = &m_clock; + storage.m_queue = m_perThreadJobQueues[i]; + } + setWorkerDirectives( WorkerThreadDirectives::kGoToSleep ); // no work for them yet + setNumThreads( m_threadSupport->getCacheFriendlyNumThreads() ); + } + + void setWorkerDirectives(WorkerThreadDirectives::Type dir) + { + m_workerDirective->setDirectiveByRange(kFirstWorkerThreadId, m_numThreads, dir); + } + + virtual int getMaxNumThreads() const BT_OVERRIDE + { + return m_maxNumThreads; + } + + virtual int getNumThreads() const BT_OVERRIDE + { + return m_numThreads; + } + + virtual void setNumThreads( int numThreads ) BT_OVERRIDE + { + m_numThreads = btMax( btMin(numThreads, int(m_maxNumThreads)), 1 ); + m_numWorkerThreads = m_numThreads - 1; + m_numActiveJobQueues = 0; + // if there is at least 1 worker, + if ( m_numWorkerThreads > 0 ) + { + // re-setup job stealing between queues to avoid attempting to steal from an inactive job queue + JobQueue* lastActiveContext = m_perThreadJobQueues[ m_numThreads - 1 ]; + int iLastActiveContext = lastActiveContext - &m_jobQueues[0]; + m_numActiveJobQueues = iLastActiveContext + 1; + for ( int i = 0; i < m_jobQueues.size(); ++i ) + { + m_jobQueues[ i ].setupJobStealing( &m_jobQueues, m_numActiveJobQueues ); + } + } + m_workerDirective->setDirectiveByRange(m_numThreads, BT_MAX_THREAD_COUNT, WorkerThreadDirectives::kGoToSleep); + } + + void waitJobs() + { + BT_PROFILE( "waitJobs" ); + // have the main thread work until the job queues are empty + int numMainThreadJobsFinished = 0; + for ( int i = 0; i < m_numActiveJobQueues; ++i ) + { + while ( IJob* job = m_jobQueues[i].consumeJob() ) + { + job->executeJob( 0 ); + numMainThreadJobsFinished++; + } + } + + // done with jobs for now, tell workers to rest (but not sleep) + setWorkerDirectives( WorkerThreadDirectives::kStayAwakeButIdle ); + + btU64 clockStart = m_clock.getTimeMicroseconds(); + // wait for workers to finish any jobs in progress + while ( true ) + { + int numWorkerJobsFinished = 0; + for ( int iThread = kFirstWorkerThreadId; iThread < m_numThreads; ++iThread ) + { + ThreadLocalStorage* storage = &m_threadLocalStorage[iThread]; + storage->m_mutex.lock(); + numWorkerJobsFinished += storage->m_numJobsFinished; + storage->m_mutex.unlock(); + } + if (numWorkerJobsFinished + numMainThreadJobsFinished == m_numJobs) + { + break; + } + btU64 timeElapsed = m_clock.getTimeMicroseconds() - clockStart; + btAssert(timeElapsed < 1000); + if (timeElapsed > 100000) + { + break; + } + btSpinPause(); + } + } + + void wakeWorkers(int numWorkersToWake) + { + BT_PROFILE( "wakeWorkers" ); + btAssert( m_workerDirective->getDirective(1) == WorkerThreadDirectives::kScanForJobs ); + int numDesiredWorkers = btMin(numWorkersToWake, m_numWorkerThreads); + int numActiveWorkers = 0; + for ( int iWorker = 0; iWorker < m_numWorkerThreads; ++iWorker ) + { + // note this count of active workers is not necessarily totally reliable, because a worker thread could be + // just about to put itself to sleep. So we may on occasion fail to wake up all the workers. It should be rare. + ThreadLocalStorage& storage = m_threadLocalStorage[ kFirstWorkerThreadId + iWorker ]; + if (storage.m_status != WorkerThreadStatus::kSleeping) + { + numActiveWorkers++; + } + } + for ( int iWorker = 0; iWorker < m_numWorkerThreads && numActiveWorkers < numDesiredWorkers; ++iWorker ) + { + ThreadLocalStorage& storage = m_threadLocalStorage[ kFirstWorkerThreadId + iWorker ]; + if (storage.m_status == WorkerThreadStatus::kSleeping) + { + m_threadSupport->runTask( iWorker, &storage ); + numActiveWorkers++; + } + } + } + + void waitForWorkersToSleep() + { + BT_PROFILE( "waitForWorkersToSleep" ); + setWorkerDirectives( WorkerThreadDirectives::kGoToSleep ); + m_threadSupport->waitForAllTasks(); + for ( int i = kFirstWorkerThreadId; i < m_numThreads; i++ ) + { + ThreadLocalStorage& storage = m_threadLocalStorage[i]; + btAssert( storage.m_status == WorkerThreadStatus::kSleeping ); + } + } + + virtual void sleepWorkerThreadsHint() BT_OVERRIDE + { + BT_PROFILE( "sleepWorkerThreadsHint" ); + // hint the task scheduler that we may not be using these threads for a little while + setWorkerDirectives( WorkerThreadDirectives::kGoToSleep ); + } + + void prepareWorkerThreads() + { + for ( int i = kFirstWorkerThreadId; i < m_numThreads; ++i ) + { + ThreadLocalStorage& storage = m_threadLocalStorage[i]; + storage.m_mutex.lock(); + storage.m_numJobsFinished = 0; + storage.m_mutex.unlock(); + } + setWorkerDirectives( WorkerThreadDirectives::kScanForJobs ); + } + + virtual void parallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) BT_OVERRIDE + { + BT_PROFILE( "parallelFor_ThreadSupport" ); + btAssert( iEnd >= iBegin ); + btAssert( grainSize >= 1 ); + int iterationCount = iEnd - iBegin; + if ( iterationCount > grainSize && m_numWorkerThreads > 0 && m_antiNestingLock.tryLock() ) + { + typedef ParallelForJob JobType; + int jobCount = ( iterationCount + grainSize - 1 ) / grainSize; + m_numJobs = jobCount; + btAssert( jobCount >= 2 ); // need more than one job for multithreading + int jobSize = sizeof( JobType ); + + for (int i = 0; i < m_numActiveJobQueues; ++i) + { + m_jobQueues[i].clearQueue( jobCount, jobSize ); + } + // prepare worker threads for incoming work + prepareWorkerThreads(); + // submit all of the jobs + int iJob = 0; + int iThread = kFirstWorkerThreadId; // first worker thread + for ( int i = iBegin; i < iEnd; i += grainSize ) + { + btAssert( iJob < jobCount ); + int iE = btMin( i + grainSize, iEnd ); + JobQueue* jq = m_perThreadJobQueues[ iThread ]; + btAssert(jq); + btAssert((jq - &m_jobQueues[0]) < m_numActiveJobQueues); + void* jobMem = jq->allocJobMem(jobSize); + JobType* job = new ( jobMem ) ParallelForJob( i, iE, body ); // placement new + jq->submitJob( job ); + iJob++; + iThread++; + if ( iThread >= m_numThreads ) + { + iThread = kFirstWorkerThreadId; // first worker thread + } + } + wakeWorkers( jobCount - 1 ); + + // put the main thread to work on emptying the job queue and then wait for all workers to finish + waitJobs(); + m_antiNestingLock.unlock(); + } + else + { + BT_PROFILE( "parallelFor_mainThread" ); + // just run on main thread + body.forLoop( iBegin, iEnd ); + } + } + virtual btScalar parallelSum( int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body ) BT_OVERRIDE + { + BT_PROFILE( "parallelSum_ThreadSupport" ); + btAssert( iEnd >= iBegin ); + btAssert( grainSize >= 1 ); + int iterationCount = iEnd - iBegin; + if ( iterationCount > grainSize && m_numWorkerThreads > 0 && m_antiNestingLock.tryLock() ) + { + typedef ParallelSumJob JobType; + int jobCount = ( iterationCount + grainSize - 1 ) / grainSize; + m_numJobs = jobCount; + btAssert( jobCount >= 2 ); // need more than one job for multithreading + int jobSize = sizeof( JobType ); + for (int i = 0; i < m_numActiveJobQueues; ++i) + { + m_jobQueues[i].clearQueue( jobCount, jobSize ); + } + + // initialize summation + for ( int iThread = 0; iThread < m_numThreads; ++iThread ) + { + m_threadLocalStorage[iThread].m_sumResult = btScalar(0); + } + + // prepare worker threads for incoming work + prepareWorkerThreads(); + // submit all of the jobs + int iJob = 0; + int iThread = kFirstWorkerThreadId; // first worker thread + for ( int i = iBegin; i < iEnd; i += grainSize ) + { + btAssert( iJob < jobCount ); + int iE = btMin( i + grainSize, iEnd ); + JobQueue* jq = m_perThreadJobQueues[ iThread ]; + btAssert(jq); + btAssert((jq - &m_jobQueues[0]) < m_numActiveJobQueues); + void* jobMem = jq->allocJobMem(jobSize); + JobType* job = new ( jobMem ) ParallelSumJob( i, iE, body, &m_threadLocalStorage[0] ); // placement new + jq->submitJob( job ); + iJob++; + iThread++; + if ( iThread >= m_numThreads ) + { + iThread = kFirstWorkerThreadId; // first worker thread + } + } + wakeWorkers( jobCount - 1 ); + + // put the main thread to work on emptying the job queue and then wait for all workers to finish + waitJobs(); + + // add up all the thread sums + btScalar sum = btScalar(0); + for ( int iThread = 0; iThread < m_numThreads; ++iThread ) + { + sum += m_threadLocalStorage[ iThread ].m_sumResult; + } + m_antiNestingLock.unlock(); + return sum; + } + else + { + BT_PROFILE( "parallelSum_mainThread" ); + // just run on main thread + return body.sumLoop( iBegin, iEnd ); + } + } +}; + + + +btITaskScheduler* btCreateDefaultTaskScheduler() +{ + btTaskSchedulerDefault* ts = new btTaskSchedulerDefault(); + ts->init(); + return ts; +} + +#else // #if BT_THREADSAFE + +btITaskScheduler* btCreateDefaultTaskScheduler() +{ + return NULL; +} + +#endif // #else // #if BT_THREADSAFE diff --git a/extern/bullet/src/LinearMath/TaskScheduler/btThreadSupportInterface.h b/extern/bullet/src/LinearMath/TaskScheduler/btThreadSupportInterface.h new file mode 100644 index 000000000000..a0ad802b1e12 --- /dev/null +++ b/extern/bullet/src/LinearMath/TaskScheduler/btThreadSupportInterface.h @@ -0,0 +1,70 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2018 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_THREAD_SUPPORT_INTERFACE_H +#define BT_THREAD_SUPPORT_INTERFACE_H + + + +class btCriticalSection +{ +public: + btCriticalSection() {} + virtual ~btCriticalSection() {} + + virtual void lock() = 0; + virtual void unlock() = 0; +}; + + +class btThreadSupportInterface +{ +public: + + virtual ~btThreadSupportInterface() {} + + virtual int getNumWorkerThreads() const = 0; // number of worker threads (total number of logical processors - 1) + virtual int getCacheFriendlyNumThreads() const = 0; // the number of logical processors sharing a single L3 cache + virtual int getLogicalToPhysicalCoreRatio() const = 0; // the number of logical processors per physical processor (usually 1 or 2) + virtual void runTask( int threadIndex, void* userData ) = 0; + virtual void waitForAllTasks() = 0; + + virtual btCriticalSection* createCriticalSection() = 0; + virtual void deleteCriticalSection( btCriticalSection* criticalSection ) = 0; + + typedef void( *ThreadFunc )( void* userPtr ); + + struct ConstructionInfo + { + ConstructionInfo( const char* uniqueName, + ThreadFunc userThreadFunc, + int threadStackSize = 65535 + ) + :m_uniqueName( uniqueName ), + m_userThreadFunc( userThreadFunc ), + m_threadStackSize( threadStackSize ) + { + } + + const char* m_uniqueName; + ThreadFunc m_userThreadFunc; + int m_threadStackSize; + }; + + static btThreadSupportInterface* create( const ConstructionInfo& info ); +}; + +#endif //BT_THREAD_SUPPORT_INTERFACE_H + diff --git a/extern/bullet/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp b/extern/bullet/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp new file mode 100644 index 000000000000..ccd7d1e1213a --- /dev/null +++ b/extern/bullet/src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp @@ -0,0 +1,364 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2018 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. +*/ + + +#if BT_THREADSAFE && !defined( _WIN32 ) + + +#include "LinearMath/btScalar.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btThreads.h" +#include "LinearMath/btMinMax.h" +#include "btThreadSupportInterface.h" + +#include +#include +#include + + +#ifndef _XOPEN_SOURCE +#define _XOPEN_SOURCE 600 //for definition of pthread_barrier_t, see http://pages.cs.wisc.edu/~travitch/pthreads_primer.html +#endif //_XOPEN_SOURCE +#include +#include +#include //for sysconf + + +/// +/// getNumHardwareThreads() +/// +/// +/// https://stackoverflow.com/questions/150355/programmatically-find-the-number-of-cores-on-a-machine +/// +#if __cplusplus >= 201103L + +#include + +int btGetNumHardwareThreads() +{ + return std::thread::hardware_concurrency(); +} + +#else + +int btGetNumHardwareThreads() +{ + return sysconf( _SC_NPROCESSORS_ONLN ); +} + +#endif + + +// btThreadSupportPosix helps to initialize/shutdown libspe2, start/stop SPU tasks and communication +class btThreadSupportPosix : public btThreadSupportInterface +{ +public: + struct btThreadStatus + { + int m_taskId; + int m_commandId; + int m_status; + + ThreadFunc m_userThreadFunc; + void* m_userPtr; //for taskDesc etc + + pthread_t thread; + //each tread will wait until this signal to start its work + sem_t* startSemaphore; + + // this is a copy of m_mainSemaphore, + //each tread will signal once it is finished with its work + sem_t* m_mainSemaphore; + unsigned long threadUsed; + }; +private: + typedef unsigned long long UINT64; + + btAlignedObjectArray m_activeThreadStatus; + // m_mainSemaphoresemaphore will signal, if and how many threads are finished with their work + sem_t* m_mainSemaphore; + int m_numThreads; + UINT64 m_startedThreadsMask; + void startThreads( const ConstructionInfo& threadInfo ); + void stopThreads(); + int waitForResponse(); + +public: + btThreadSupportPosix( const ConstructionInfo& threadConstructionInfo ); + virtual ~btThreadSupportPosix(); + + virtual int getNumWorkerThreads() const BT_OVERRIDE { return m_numThreads; } + // TODO: return the number of logical processors sharing the first L3 cache + virtual int getCacheFriendlyNumThreads() const BT_OVERRIDE { return m_numThreads + 1; } + // TODO: detect if CPU has hyperthreading enabled + virtual int getLogicalToPhysicalCoreRatio() const BT_OVERRIDE { return 1; } + + virtual void runTask( int threadIndex, void* userData ) BT_OVERRIDE; + virtual void waitForAllTasks() BT_OVERRIDE; + + virtual btCriticalSection* createCriticalSection() BT_OVERRIDE; + virtual void deleteCriticalSection( btCriticalSection* criticalSection ) BT_OVERRIDE; +}; + + +#define checkPThreadFunction(returnValue) \ + if(0 != returnValue) { \ + printf("PThread problem at line %i in file %s: %i %d\n", __LINE__, __FILE__, returnValue, errno); \ + } + +// The number of threads should be equal to the number of available cores +// Todo: each worker should be linked to a single core, using SetThreadIdealProcessor. + + +btThreadSupportPosix::btThreadSupportPosix( const ConstructionInfo& threadConstructionInfo ) +{ + startThreads( threadConstructionInfo ); +} + +// cleanup/shutdown Libspe2 +btThreadSupportPosix::~btThreadSupportPosix() +{ + stopThreads(); +} + +#if (defined (__APPLE__)) +#define NAMED_SEMAPHORES +#endif + + +static sem_t* createSem( const char* baseName ) +{ + static int semCount = 0; +#ifdef NAMED_SEMAPHORES + /// Named semaphore begin + char name[ 32 ]; + snprintf( name, 32, "/%8.s-%4.d-%4.4d", baseName, getpid(), semCount++ ); + sem_t* tempSem = sem_open( name, O_CREAT, 0600, 0 ); + + if ( tempSem != reinterpret_cast( SEM_FAILED ) ) + { + // printf("Created \"%s\" Semaphore %p\n", name, tempSem); + } + else + { + //printf("Error creating Semaphore %d\n", errno); + exit( -1 ); + } + /// Named semaphore end +#else + sem_t* tempSem = new sem_t; + checkPThreadFunction( sem_init( tempSem, 0, 0 ) ); +#endif + return tempSem; +} + +static void destroySem( sem_t* semaphore ) +{ +#ifdef NAMED_SEMAPHORES + checkPThreadFunction( sem_close( semaphore ) ); +#else + checkPThreadFunction( sem_destroy( semaphore ) ); + delete semaphore; +#endif +} + +static void *threadFunction( void *argument ) +{ + btThreadSupportPosix::btThreadStatus* status = ( btThreadSupportPosix::btThreadStatus* )argument; + + while ( 1 ) + { + checkPThreadFunction( sem_wait( status->startSemaphore ) ); + void* userPtr = status->m_userPtr; + + if ( userPtr ) + { + btAssert( status->m_status ); + status->m_userThreadFunc( userPtr ); + status->m_status = 2; + checkPThreadFunction( sem_post( status->m_mainSemaphore ) ); + status->threadUsed++; + } + else + { + //exit Thread + status->m_status = 3; + checkPThreadFunction( sem_post( status->m_mainSemaphore ) ); + printf( "Thread with taskId %i exiting\n", status->m_taskId ); + break; + } + } + + printf( "Thread TERMINATED\n" ); +} + +///send messages to SPUs +void btThreadSupportPosix::runTask( int threadIndex, void* userData ) +{ + ///we should spawn an SPU task here, and in 'waitForResponse' it should wait for response of the (one of) the first tasks that finished + btThreadStatus& threadStatus = m_activeThreadStatus[ threadIndex ]; + btAssert( threadIndex >= 0 ); + btAssert( threadIndex < m_activeThreadStatus.size() ); + + threadStatus.m_commandId = 1; + threadStatus.m_status = 1; + threadStatus.m_userPtr = userData; + m_startedThreadsMask |= UINT64( 1 ) << threadIndex; + + // fire event to start new task + checkPThreadFunction( sem_post( threadStatus.startSemaphore ) ); +} + + +///check for messages from SPUs +int btThreadSupportPosix::waitForResponse() +{ + ///We should wait for (one of) the first tasks to finish (or other SPU messages), and report its response + ///A possible response can be 'yes, SPU handled it', or 'no, please do a PPU fallback' + + btAssert( m_activeThreadStatus.size() ); + + // wait for any of the threads to finish + checkPThreadFunction( sem_wait( m_mainSemaphore ) ); + // get at least one thread which has finished + size_t last = -1; + + for ( size_t t = 0; t < size_t( m_activeThreadStatus.size() ); ++t ) + { + if ( 2 == m_activeThreadStatus[ t ].m_status ) + { + last = t; + break; + } + } + + btThreadStatus& threadStatus = m_activeThreadStatus[ last ]; + + btAssert( threadStatus.m_status > 1 ); + threadStatus.m_status = 0; + + // need to find an active spu + btAssert( last >= 0 ); + m_startedThreadsMask &= ~( UINT64( 1 ) << last ); + + return last; +} + + +void btThreadSupportPosix::waitForAllTasks() +{ + while ( m_startedThreadsMask ) + { + waitForResponse(); + } +} + + +void btThreadSupportPosix::startThreads( const ConstructionInfo& threadConstructionInfo ) +{ + m_numThreads = btGetNumHardwareThreads() - 1; // main thread exists already + printf( "%s creating %i threads.\n", __FUNCTION__, m_numThreads ); + m_activeThreadStatus.resize( m_numThreads ); + m_startedThreadsMask = 0; + + m_mainSemaphore = createSem( "main" ); + //checkPThreadFunction(sem_wait(mainSemaphore)); + + for ( int i = 0; i < m_numThreads; i++ ) + { + printf( "starting thread %d\n", i ); + btThreadStatus& threadStatus = m_activeThreadStatus[ i ]; + threadStatus.startSemaphore = createSem( "threadLocal" ); + checkPThreadFunction( pthread_create( &threadStatus.thread, NULL, &threadFunction, (void*) &threadStatus ) ); + + threadStatus.m_userPtr = 0; + threadStatus.m_taskId = i; + threadStatus.m_commandId = 0; + threadStatus.m_status = 0; + threadStatus.m_mainSemaphore = m_mainSemaphore; + threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc; + threadStatus.threadUsed = 0; + + printf( "started thread %d \n", i ); + } +} + +///tell the task scheduler we are done with the SPU tasks +void btThreadSupportPosix::stopThreads() +{ + for ( size_t t = 0; t < size_t( m_activeThreadStatus.size() ); ++t ) + { + btThreadStatus& threadStatus = m_activeThreadStatus[ t ]; + printf( "%s: Thread %i used: %ld\n", __FUNCTION__, int( t ), threadStatus.threadUsed ); + + threadStatus.m_userPtr = 0; + checkPThreadFunction( sem_post( threadStatus.startSemaphore ) ); + checkPThreadFunction( sem_wait( m_mainSemaphore ) ); + + printf( "destroy semaphore\n" ); + destroySem( threadStatus.startSemaphore ); + printf( "semaphore destroyed\n" ); + checkPThreadFunction( pthread_join( threadStatus.thread, 0 ) ); + + } + printf( "destroy main semaphore\n" ); + destroySem( m_mainSemaphore ); + printf( "main semaphore destroyed\n" ); + m_activeThreadStatus.clear(); +} + +class btCriticalSectionPosix : public btCriticalSection +{ + pthread_mutex_t m_mutex; + +public: + btCriticalSectionPosix() + { + pthread_mutex_init( &m_mutex, NULL ); + } + virtual ~btCriticalSectionPosix() + { + pthread_mutex_destroy( &m_mutex ); + } + + virtual void lock() + { + pthread_mutex_lock( &m_mutex ); + } + virtual void unlock() + { + pthread_mutex_unlock( &m_mutex ); + } +}; + + +btCriticalSection* btThreadSupportPosix::createCriticalSection() +{ + return new btCriticalSectionPosix(); +} + +void btThreadSupportPosix::deleteCriticalSection( btCriticalSection* cs ) +{ + delete cs; +} + + +btThreadSupportInterface* btThreadSupportInterface::create( const ConstructionInfo& info ) +{ + return new btThreadSupportPosix( info ); +} + +#endif // BT_THREADSAFE && !defined( _WIN32 ) + diff --git a/extern/bullet/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp b/extern/bullet/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp new file mode 100644 index 000000000000..00edac650b43 --- /dev/null +++ b/extern/bullet/src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp @@ -0,0 +1,472 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2018 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. +*/ + +#if defined( _WIN32 ) && BT_THREADSAFE + +#include "LinearMath/btScalar.h" +#include "LinearMath/btMinMax.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btThreads.h" +#include "btThreadSupportInterface.h" +#include +#include + + +struct btProcessorInfo +{ + int numLogicalProcessors; + int numCores; + int numNumaNodes; + int numL1Cache; + int numL2Cache; + int numL3Cache; + int numPhysicalPackages; + static const int maxNumTeamMasks = 32; + int numTeamMasks; + UINT64 processorTeamMasks[ maxNumTeamMasks ]; +}; + +UINT64 getProcessorTeamMask( const btProcessorInfo& procInfo, int procId ) +{ + UINT64 procMask = UINT64( 1 ) << procId; + for ( int i = 0; i < procInfo.numTeamMasks; ++i ) + { + if ( procMask & procInfo.processorTeamMasks[ i ] ) + { + return procInfo.processorTeamMasks[ i ]; + } + } + return 0; +} + +int getProcessorTeamIndex( const btProcessorInfo& procInfo, int procId ) +{ + UINT64 procMask = UINT64( 1 ) << procId; + for ( int i = 0; i < procInfo.numTeamMasks; ++i ) + { + if ( procMask & procInfo.processorTeamMasks[ i ] ) + { + return i; + } + } + return -1; +} + +int countSetBits( ULONG64 bits ) +{ + int count = 0; + while ( bits ) + { + if ( bits & 1 ) + { + count++; + } + bits >>= 1; + } + return count; +} + + +typedef BOOL( WINAPI *Pfn_GetLogicalProcessorInformation )( PSYSTEM_LOGICAL_PROCESSOR_INFORMATION, PDWORD ); + + +void getProcessorInformation( btProcessorInfo* procInfo ) +{ + memset( procInfo, 0, sizeof( *procInfo ) ); + Pfn_GetLogicalProcessorInformation getLogicalProcInfo = + (Pfn_GetLogicalProcessorInformation) GetProcAddress( GetModuleHandle( TEXT( "kernel32" ) ), "GetLogicalProcessorInformation" ); + if ( getLogicalProcInfo == NULL ) + { + // no info + return; + } + PSYSTEM_LOGICAL_PROCESSOR_INFORMATION buf = NULL; + DWORD bufSize = 0; + while ( true ) + { + if ( getLogicalProcInfo( buf, &bufSize ) ) + { + break; + } + else + { + if ( GetLastError() == ERROR_INSUFFICIENT_BUFFER ) + { + if ( buf ) + { + free( buf ); + } + buf = (PSYSTEM_LOGICAL_PROCESSOR_INFORMATION) malloc( bufSize ); + } + } + } + + int len = bufSize / sizeof( *buf ); + for ( int i = 0; i < len; ++i ) + { + PSYSTEM_LOGICAL_PROCESSOR_INFORMATION info = buf + i; + switch ( info->Relationship ) + { + case RelationNumaNode: + procInfo->numNumaNodes++; + break; + + case RelationProcessorCore: + procInfo->numCores++; + procInfo->numLogicalProcessors += countSetBits( info->ProcessorMask ); + break; + + case RelationCache: + if ( info->Cache.Level == 1 ) + { + procInfo->numL1Cache++; + } + else if ( info->Cache.Level == 2 ) + { + procInfo->numL2Cache++; + } + else if ( info->Cache.Level == 3 ) + { + procInfo->numL3Cache++; + // processors that share L3 cache are considered to be on the same team + // because they can more easily work together on the same data. + // Large performance penalties will occur if 2 or more threads from different + // teams attempt to frequently read and modify the same cache lines. + // + // On the AMD Ryzen 7 CPU for example, the 8 cores on the CPU are split into + // 2 CCX units of 4 cores each. Each CCX has a separate L3 cache, so if both + // CCXs are operating on the same data, many cycles will be spent keeping the + // two caches coherent. + if ( procInfo->numTeamMasks < btProcessorInfo::maxNumTeamMasks ) + { + procInfo->processorTeamMasks[ procInfo->numTeamMasks ] = info->ProcessorMask; + procInfo->numTeamMasks++; + } + } + break; + + case RelationProcessorPackage: + procInfo->numPhysicalPackages++; + break; + } + } + free( buf ); +} + + + +///btThreadSupportWin32 helps to initialize/shutdown libspe2, start/stop SPU tasks and communication +class btThreadSupportWin32 : public btThreadSupportInterface +{ +public: + struct btThreadStatus + { + int m_taskId; + int m_commandId; + int m_status; + + ThreadFunc m_userThreadFunc; + void* m_userPtr; //for taskDesc etc + + void* m_threadHandle; //this one is calling 'Win32ThreadFunc' + + void* m_eventStartHandle; + char m_eventStartHandleName[ 32 ]; + + void* m_eventCompleteHandle; + char m_eventCompleteHandleName[ 32 ]; + }; + +private: + btAlignedObjectArray m_activeThreadStatus; + btAlignedObjectArray m_completeHandles; + int m_numThreads; + DWORD_PTR m_startedThreadMask; + btProcessorInfo m_processorInfo; + + void startThreads( const ConstructionInfo& threadInfo ); + void stopThreads(); + int waitForResponse(); + +public: + + btThreadSupportWin32( const ConstructionInfo& threadConstructionInfo ); + virtual ~btThreadSupportWin32(); + + virtual int getNumWorkerThreads() const BT_OVERRIDE { return m_numThreads; } + virtual int getCacheFriendlyNumThreads() const BT_OVERRIDE { return countSetBits(m_processorInfo.processorTeamMasks[0]); } + virtual int getLogicalToPhysicalCoreRatio() const BT_OVERRIDE { return m_processorInfo.numLogicalProcessors / m_processorInfo.numCores; } + + virtual void runTask( int threadIndex, void* userData ) BT_OVERRIDE; + virtual void waitForAllTasks() BT_OVERRIDE; + + virtual btCriticalSection* createCriticalSection() BT_OVERRIDE; + virtual void deleteCriticalSection( btCriticalSection* criticalSection ) BT_OVERRIDE; +}; + + +btThreadSupportWin32::btThreadSupportWin32( const ConstructionInfo & threadConstructionInfo ) +{ + startThreads( threadConstructionInfo ); +} + + +btThreadSupportWin32::~btThreadSupportWin32() +{ + stopThreads(); +} + + +DWORD WINAPI win32threadStartFunc( LPVOID lpParam ) +{ + btThreadSupportWin32::btThreadStatus* status = ( btThreadSupportWin32::btThreadStatus* )lpParam; + + while ( 1 ) + { + WaitForSingleObject( status->m_eventStartHandle, INFINITE ); + void* userPtr = status->m_userPtr; + + if ( userPtr ) + { + btAssert( status->m_status ); + status->m_userThreadFunc( userPtr ); + status->m_status = 2; + SetEvent( status->m_eventCompleteHandle ); + } + else + { + //exit Thread + status->m_status = 3; + printf( "Thread with taskId %i with handle %p exiting\n", status->m_taskId, status->m_threadHandle ); + SetEvent( status->m_eventCompleteHandle ); + break; + } + } + printf( "Thread TERMINATED\n" ); + return 0; +} + + +void btThreadSupportWin32::runTask( int threadIndex, void* userData ) +{ + btThreadStatus& threadStatus = m_activeThreadStatus[ threadIndex ]; + btAssert( threadIndex >= 0 ); + btAssert( int( threadIndex ) < m_activeThreadStatus.size() ); + + threadStatus.m_commandId = 1; + threadStatus.m_status = 1; + threadStatus.m_userPtr = userData; + m_startedThreadMask |= DWORD_PTR( 1 ) << threadIndex; + + ///fire event to start new task + SetEvent( threadStatus.m_eventStartHandle ); +} + + +int btThreadSupportWin32::waitForResponse() +{ + btAssert( m_activeThreadStatus.size() ); + + int last = -1; + DWORD res = WaitForMultipleObjects( m_completeHandles.size(), &m_completeHandles[ 0 ], FALSE, INFINITE ); + btAssert( res != WAIT_FAILED ); + last = res - WAIT_OBJECT_0; + + btThreadStatus& threadStatus = m_activeThreadStatus[ last ]; + btAssert( threadStatus.m_threadHandle ); + btAssert( threadStatus.m_eventCompleteHandle ); + + //WaitForSingleObject(threadStatus.m_eventCompleteHandle, INFINITE); + btAssert( threadStatus.m_status > 1 ); + threadStatus.m_status = 0; + + ///need to find an active spu + btAssert( last >= 0 ); + m_startedThreadMask &= ~( DWORD_PTR( 1 ) << last ); + + return last; +} + + +void btThreadSupportWin32::waitForAllTasks() +{ + while ( m_startedThreadMask ) + { + waitForResponse(); + } +} + + +void btThreadSupportWin32::startThreads( const ConstructionInfo& threadConstructionInfo ) +{ + static int uniqueId = 0; + uniqueId++; + btProcessorInfo& procInfo = m_processorInfo; + getProcessorInformation( &procInfo ); + DWORD_PTR dwProcessAffinityMask = 0; + DWORD_PTR dwSystemAffinityMask = 0; + if ( !GetProcessAffinityMask( GetCurrentProcess(), &dwProcessAffinityMask, &dwSystemAffinityMask ) ) + { + dwProcessAffinityMask = 0; + } + ///The number of threads should be equal to the number of available cores - 1 + m_numThreads = btMin(procInfo.numLogicalProcessors, int(BT_MAX_THREAD_COUNT)) - 1; // cap to max thread count (-1 because main thread already exists) + + m_activeThreadStatus.resize( m_numThreads ); + m_completeHandles.resize( m_numThreads ); + m_startedThreadMask = 0; + + // set main thread affinity + if ( DWORD_PTR mask = dwProcessAffinityMask & getProcessorTeamMask( procInfo, 0 )) + { + SetThreadAffinityMask( GetCurrentThread(), mask ); + SetThreadIdealProcessor( GetCurrentThread(), 0 ); + } + + for ( int i = 0; i < m_numThreads; i++ ) + { + printf( "starting thread %d\n", i ); + + btThreadStatus& threadStatus = m_activeThreadStatus[ i ]; + + LPSECURITY_ATTRIBUTES lpThreadAttributes = NULL; + SIZE_T dwStackSize = threadConstructionInfo.m_threadStackSize; + LPTHREAD_START_ROUTINE lpStartAddress = &win32threadStartFunc; + LPVOID lpParameter = &threadStatus; + DWORD dwCreationFlags = 0; + LPDWORD lpThreadId = 0; + + threadStatus.m_userPtr = 0; + + sprintf( threadStatus.m_eventStartHandleName, "es%.8s%d%d", threadConstructionInfo.m_uniqueName, uniqueId, i ); + threadStatus.m_eventStartHandle = CreateEventA( 0, false, false, threadStatus.m_eventStartHandleName ); + + sprintf( threadStatus.m_eventCompleteHandleName, "ec%.8s%d%d", threadConstructionInfo.m_uniqueName, uniqueId, i ); + threadStatus.m_eventCompleteHandle = CreateEventA( 0, false, false, threadStatus.m_eventCompleteHandleName ); + + m_completeHandles[ i ] = threadStatus.m_eventCompleteHandle; + + HANDLE handle = CreateThread( lpThreadAttributes, dwStackSize, lpStartAddress, lpParameter, dwCreationFlags, lpThreadId ); + //SetThreadPriority( handle, THREAD_PRIORITY_HIGHEST ); + // highest priority -- can cause erratic performance when numThreads > numCores + // we don't want worker threads to be higher priority than the main thread or the main thread could get + // totally shut out and unable to tell the workers to stop + //SetThreadPriority( handle, THREAD_PRIORITY_BELOW_NORMAL ); + + { + int processorId = i + 1; // leave processor 0 for main thread + DWORD_PTR teamMask = getProcessorTeamMask( procInfo, processorId ); + if ( teamMask ) + { + // bind each thread to only execute on processors of it's assigned team + // - for single-socket Intel x86 CPUs this has no effect (only a single, shared L3 cache so there is only 1 team) + // - for multi-socket Intel this will keep threads from migrating from one socket to another + // - for AMD Ryzen this will keep threads from migrating from one CCX to another + DWORD_PTR mask = teamMask & dwProcessAffinityMask; + if ( mask ) + { + SetThreadAffinityMask( handle, mask ); + } + } + SetThreadIdealProcessor( handle, processorId ); + } + + threadStatus.m_taskId = i; + threadStatus.m_commandId = 0; + threadStatus.m_status = 0; + threadStatus.m_threadHandle = handle; + threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc; + + printf( "started %s thread %d with threadHandle %p\n", threadConstructionInfo.m_uniqueName, i, handle ); + } +} + +///tell the task scheduler we are done with the SPU tasks +void btThreadSupportWin32::stopThreads() +{ + for ( int i = 0; i < m_activeThreadStatus.size(); i++ ) + { + btThreadStatus& threadStatus = m_activeThreadStatus[ i ]; + if ( threadStatus.m_status > 0 ) + { + WaitForSingleObject( threadStatus.m_eventCompleteHandle, INFINITE ); + } + + threadStatus.m_userPtr = NULL; + SetEvent( threadStatus.m_eventStartHandle ); + WaitForSingleObject( threadStatus.m_eventCompleteHandle, INFINITE ); + + CloseHandle( threadStatus.m_eventCompleteHandle ); + CloseHandle( threadStatus.m_eventStartHandle ); + CloseHandle( threadStatus.m_threadHandle ); + + } + + m_activeThreadStatus.clear(); + m_completeHandles.clear(); +} + + +class btWin32CriticalSection : public btCriticalSection +{ +private: + CRITICAL_SECTION mCriticalSection; + +public: + btWin32CriticalSection() + { + InitializeCriticalSection( &mCriticalSection ); + } + + ~btWin32CriticalSection() + { + DeleteCriticalSection( &mCriticalSection ); + } + + void lock() + { + EnterCriticalSection( &mCriticalSection ); + } + + void unlock() + { + LeaveCriticalSection( &mCriticalSection ); + } +}; + + +btCriticalSection* btThreadSupportWin32::createCriticalSection() +{ + unsigned char* mem = (unsigned char*) btAlignedAlloc( sizeof( btWin32CriticalSection ), 16 ); + btWin32CriticalSection* cs = new( mem ) btWin32CriticalSection(); + return cs; +} + +void btThreadSupportWin32::deleteCriticalSection( btCriticalSection* criticalSection ) +{ + criticalSection->~btCriticalSection(); + btAlignedFree( criticalSection ); +} + + +btThreadSupportInterface* btThreadSupportInterface::create( const ConstructionInfo& info ) +{ + return new btThreadSupportWin32( info ); +} + + + +#endif //defined(_WIN32) && BT_THREADSAFE + diff --git a/extern/bullet2/src/LinearMath/btAabbUtil2.h b/extern/bullet/src/LinearMath/btAabbUtil2.h similarity index 100% rename from extern/bullet2/src/LinearMath/btAabbUtil2.h rename to extern/bullet/src/LinearMath/btAabbUtil2.h diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp b/extern/bullet/src/LinearMath/btAlignedAllocator.cpp similarity index 62% rename from extern/bullet2/src/LinearMath/btAlignedAllocator.cpp rename to extern/bullet/src/LinearMath/btAlignedAllocator.cpp index a65296c6abe1..e5f6040c4382 100644 --- a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp +++ b/extern/bullet/src/LinearMath/btAlignedAllocator.cpp @@ -105,30 +105,94 @@ void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc) } #ifdef BT_DEBUG_MEMORY_ALLOCATIONS + +static int allocations_id[10241024]; +static int allocations_bytes[10241024]; +static int mynumallocs = 0; +#include + +int btDumpMemoryLeaks() +{ + int totalLeak = 0; + + for (int i=0;i +struct btDebugPtrMagic +{ + union + { + void** vptrptr; + void* vptr; + int* iptr; + char* cptr; + }; +}; + + void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename) { + if (size==0) + { + printf("Whaat? size==0"); + return 0; + } + static int allocId = 0; + void *ret; char *real; +// to find some particular memory leak, you could do something like this: +// if (allocId==172) +// { +// printf("catch me!\n"); +// } +// if (size>1024*1024) +// { +// printf("big alloc!%d\n", size); +// } + gTotalBytesAlignedAllocs += size; gNumAlignedAllocs++; - real = (char *)sAllocFunc(size + 2*sizeof(void *) + (alignment-1)); +int sz4prt = 4*sizeof(void *); + + real = (char *)sAllocFunc(size + sz4prt + (alignment-1)); if (real) { - ret = (void*) btAlignPointer(real + 2*sizeof(void *), alignment); - *((void **)(ret)-1) = (void *)(real); - *((int*)(ret)-2) = size; + + ret = (void*) btAlignPointer(real + sz4prt, alignment); + btDebugPtrMagic p; + p.vptr = ret; + p.cptr-=sizeof(void*); + *p.vptrptr = (void*)real; + p.cptr-=sizeof(void*); + *p.iptr = size; + p.cptr-=sizeof(void*); + *p.iptr = allocId; + + allocations_id[mynumallocs] = allocId; + allocations_bytes[mynumallocs] = size; + mynumallocs++; } else { ret = (void *)(real);//?? } - printf("allocation#%d at address %x, from %s,line %d, size %d\n",gNumAlignedAllocs,real, filename,line,size); - + printf("allocation %d at address %x, from %s,line %d, size %d (total allocated = %d)\n",allocId,real, filename,line,size,gTotalBytesAlignedAllocs); + allocId++; + int* ptr = (int*)ret; *ptr = 12; return (ret); @@ -138,19 +202,43 @@ 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); + gNumAlignedFree++; + + btDebugPtrMagic p; + p.vptr = ptr; + p.cptr-=sizeof(void*); + real = *p.vptrptr; + p.cptr-=sizeof(void*); + int size = *p.iptr; + p.cptr-=sizeof(void*); + int allocId = *p.iptr; + + bool found = false; + + for (int i=0;iedges = NULL; + v->next = v; + v->prev = v; + + result.minXy = v; + result.maxXy = v; + result.minYx = v; + result.maxYx = v; + } + + return; } - // lint -fallthrough + case 1: { Vertex* v = originalVertices[start]; diff --git a/extern/bullet2/src/LinearMath/btConvexHullComputer.h b/extern/bullet/src/LinearMath/btConvexHullComputer.h similarity index 100% rename from extern/bullet2/src/LinearMath/btConvexHullComputer.h rename to extern/bullet/src/LinearMath/btConvexHullComputer.h diff --git a/extern/bullet2/src/LinearMath/btCpuFeatureUtility.h b/extern/bullet/src/LinearMath/btCpuFeatureUtility.h similarity index 100% rename from extern/bullet2/src/LinearMath/btCpuFeatureUtility.h rename to extern/bullet/src/LinearMath/btCpuFeatureUtility.h diff --git a/extern/bullet2/src/LinearMath/btDefaultMotionState.h b/extern/bullet/src/LinearMath/btDefaultMotionState.h similarity index 100% rename from extern/bullet2/src/LinearMath/btDefaultMotionState.h rename to extern/bullet/src/LinearMath/btDefaultMotionState.h diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp b/extern/bullet/src/LinearMath/btGeometryUtil.cpp similarity index 100% rename from extern/bullet2/src/LinearMath/btGeometryUtil.cpp rename to extern/bullet/src/LinearMath/btGeometryUtil.cpp diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.h b/extern/bullet/src/LinearMath/btGeometryUtil.h similarity index 100% rename from extern/bullet2/src/LinearMath/btGeometryUtil.h rename to extern/bullet/src/LinearMath/btGeometryUtil.h diff --git a/extern/bullet2/src/LinearMath/btGrahamScan2dConvexHull.h b/extern/bullet/src/LinearMath/btGrahamScan2dConvexHull.h similarity index 100% rename from extern/bullet2/src/LinearMath/btGrahamScan2dConvexHull.h rename to extern/bullet/src/LinearMath/btGrahamScan2dConvexHull.h diff --git a/extern/bullet2/src/LinearMath/btHashMap.h b/extern/bullet/src/LinearMath/btHashMap.h similarity index 89% rename from extern/bullet2/src/LinearMath/btHashMap.h rename to extern/bullet/src/LinearMath/btHashMap.h index af9727b7adaf..180e7b44afad 100644 --- a/extern/bullet2/src/LinearMath/btHashMap.h +++ b/extern/bullet/src/LinearMath/btHashMap.h @@ -17,12 +17,13 @@ subject to the following restrictions: #ifndef BT_HASH_MAP_H #define BT_HASH_MAP_H +#include #include "btAlignedObjectArray.h" ///very basic hashable string implementation, compatible with btHashMap struct btHashString { - const char* m_string; + std::string m_string1; unsigned int m_hash; SIMD_FORCE_INLINE unsigned int getHash()const @@ -30,8 +31,13 @@ struct btHashString return m_hash; } + btHashString() + { + m_string1=""; + m_hash=0; + } btHashString(const char* name) - :m_string(name) + :m_string1(name) { /* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */ static const unsigned int InitialFNV = 2166136261u; @@ -40,36 +46,18 @@ struct btHashString /* Fowler / Noll / Vo (FNV) Hash */ unsigned int hash = InitialFNV; - for(int i = 0; m_string[i]; i++) + for(int i = 0; m_string1.c_str()[i]; i++) { - hash = hash ^ (m_string[i]); /* xor the low 8 bits */ + hash = hash ^ (m_string1.c_str()[i]); /* xor the low 8 bits */ hash = hash * FNVMultiple; /* multiply by the magic number */ } m_hash = hash; } - int portableStringCompare(const char* src, const char* dst) const - { - int ret = 0 ; - - while( ! (ret = *(unsigned char *)src - *(unsigned char *)dst) && *dst) - ++src, ++dst; - - if ( ret < 0 ) - ret = -1 ; - else if ( ret > 0 ) - ret = 1 ; - - return( ret ); - } - bool equals(const btHashString& other) const { - return (m_string == other.m_string) || - (0==portableStringCompare(m_string,other.m_string)); - + return (m_string1 == other.m_string1); } - }; const int BT_HASH_NULL=0xffffffff; @@ -79,6 +67,11 @@ class btHashInt { int m_uid; public: + + btHashInt() + { + } + btHashInt(int uid) :m_uid(uid) { } @@ -100,9 +93,10 @@ class btHashInt //to our success SIMD_FORCE_INLINE unsigned int getHash()const { - int key = m_uid; + unsigned int key = m_uid; // Thomas Wang's hash key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); + return key; } }; @@ -115,7 +109,7 @@ class btHashPtr union { const void* m_pointer; - int m_hashValues[2]; + unsigned int m_hashValues[2]; }; public: @@ -140,8 +134,7 @@ class btHashPtr { const bool VOID_IS_8 = ((sizeof(void*)==8)); - int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0]; - + unsigned int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0]; // Thomas Wang's hash key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); return key; @@ -174,7 +167,7 @@ class btHashKeyPtr //to our success SIMD_FORCE_INLINE unsigned int getHash()const { - int key = m_uid; + unsigned int key = m_uid; // Thomas Wang's hash key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); return key; @@ -206,7 +199,7 @@ class btHashKey //to our success SIMD_FORCE_INLINE unsigned int getHash()const { - int key = m_uid; + unsigned int key = m_uid; // Thomas Wang's hash key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16); return key; @@ -384,17 +377,40 @@ class btHashMap const Value* getAtIndex(int index) const { btAssert(index < m_valueArray.size()); - - return &m_valueArray[index]; + btAssert(index>=0); + if (index>=0 && index < m_valueArray.size()) + { + return &m_valueArray[index]; + } + return 0; } Value* getAtIndex(int index) { btAssert(index < m_valueArray.size()); - - return &m_valueArray[index]; + btAssert(index>=0); + if (index>=0 && index < m_valueArray.size()) + { + return &m_valueArray[index]; + } + return 0; } + Key getKeyAtIndex(int index) + { + btAssert(index < m_keyArray.size()); + btAssert(index>=0); + return m_keyArray[index]; + } + + const Key getKeyAtIndex(int index) const + { + btAssert(index < m_keyArray.size()); + btAssert(index>=0); + return m_keyArray[index]; + } + + Value* operator[](const Key& key) { return find(key); } diff --git a/extern/bullet2/src/LinearMath/btIDebugDraw.h b/extern/bullet/src/LinearMath/btIDebugDraw.h similarity index 99% rename from extern/bullet2/src/LinearMath/btIDebugDraw.h rename to extern/bullet/src/LinearMath/btIDebugDraw.h index 58c9838c4903..936aaa896b07 100644 --- a/extern/bullet2/src/LinearMath/btIDebugDraw.h +++ b/extern/bullet/src/LinearMath/btIDebugDraw.h @@ -166,9 +166,9 @@ class btIDebugDraw virtual void drawTransform(const btTransform& transform, btScalar orthoLen) { btVector3 start = transform.getOrigin(); - drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0)); - drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0)); - drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f)); + drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(1.f,0.3,0.3)); + drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0.3,1.f, 0.3)); + drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0.3, 0.3,1.f)); } virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle, @@ -469,6 +469,10 @@ class btIDebugDraw drawLine(transform*pt2,transform*pt3,color); } + virtual void clearLines() + { + } + virtual void flushLines() { } diff --git a/extern/bullet2/src/LinearMath/btList.h b/extern/bullet/src/LinearMath/btList.h similarity index 100% rename from extern/bullet2/src/LinearMath/btList.h rename to extern/bullet/src/LinearMath/btList.h diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet/src/LinearMath/btMatrix3x3.h similarity index 96% rename from extern/bullet2/src/LinearMath/btMatrix3x3.h rename to extern/bullet/src/LinearMath/btMatrix3x3.h index 41dea6948352..6cc4993da56e 100644 --- a/extern/bullet2/src/LinearMath/btMatrix3x3.h +++ b/extern/bullet/src/LinearMath/btMatrix3x3.h @@ -289,7 +289,7 @@ ATTRIBUTE_ALIGNED16(class) btMatrix3x3 { /** @brief Set the matrix from euler angles YPR around ZYX axes * @param eulerX Roll about X axis * @param eulerY Pitch around Y axis - * @param eulerZ Yaw aboud Z axis + * @param eulerZ Yaw about Z axis * * These angles are used to produce a rotation matrix. The euler * angles are applied in ZYX order. I.e a vector is first rotated @@ -514,7 +514,7 @@ ATTRIBUTE_ALIGNED16(class) btMatrix3x3 { /**@brief Get the matrix represented as euler angles around ZYX - * @param yaw Yaw around X axis + * @param yaw Yaw around Z axis * @param pitch Pitch around Y axis * @param roll around X axis * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/ @@ -647,15 +647,45 @@ ATTRIBUTE_ALIGNED16(class) btMatrix3x3 { return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); } + ///extractRotation is from "A robust method to extract the rotational part of deformations" + ///See http://dl.acm.org/citation.cfm?doid=2994258.2994269 + ///decomposes a matrix A in a orthogonal matrix R and a + ///symmetric matrix S: + ///A = R*S. + ///note that R can include both rotation and scaling. + SIMD_FORCE_INLINE void extractRotation(btQuaternion &q,btScalar tolerance = 1.0e-9, int maxIter=100) + { + int iter =0; + btScalar w; + const btMatrix3x3& A=*this; + for(iter = 0; iter < maxIter; iter++) + { + btMatrix3x3 R(q); + btVector3 omega = (R.getColumn(0).cross(A.getColumn(0)) + R.getColumn(1).cross(A.getColumn(1)) + + R.getColumn(2).cross(A.getColumn(2)) + ) * (btScalar(1.0) / btFabs(R.getColumn(0).dot(A.getColumn(0)) + R.getColumn + (1).dot(A.getColumn(1)) + R.getColumn(2).dot(A.getColumn(2))) + + tolerance); + w = omega.norm(); + if(w < tolerance) + break; + q = btQuaternion(btVector3((btScalar(1.0) / w) * omega),w) * + q; + q.normalize(); + } + } + + + /**@brief diagonalizes this matrix by the Jacobi method. * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original - * coordinate system, i.e., old_this = rot * new_this * rot^T. + * coordinate system, i.e., old_this = rot * new_this * rot^T. * @param threshold See iteration - * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied - * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. - * - * Note that this matrix is assumed to be symmetric. + * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied + * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. + * + * Note that this matrix is assumed to be symmetric. */ void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) { @@ -737,7 +767,6 @@ ATTRIBUTE_ALIGNED16(class) btMatrix3x3 { - /**@brief Calculate the matrix cofactor * @param r1 The first row to use for calculating the cofactor * @param c1 The first column to use for calculating the cofactor @@ -1047,7 +1076,8 @@ btMatrix3x3::inverse() const { btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); btScalar det = (*this)[0].dot(co); - btFullAssert(det != btScalar(0.0)); + //btFullAssert(det != btScalar(0.0)); + btAssert(det != btScalar(0.0)); btScalar s = btScalar(1.0) / det; return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, @@ -1329,7 +1359,9 @@ SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2) c0 = _mm_and_ps(c0, c1); c0 = _mm_and_ps(c0, c2); - return (0x7 == _mm_movemask_ps((__m128)c0)); + int m = _mm_movemask_ps((__m128)c0); + return (0x7 == (m & 0x7)); + #else return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] && diff --git a/extern/bullet2/src/LinearMath/btMatrixX.h b/extern/bullet/src/LinearMath/btMatrixX.h similarity index 100% rename from extern/bullet2/src/LinearMath/btMatrixX.h rename to extern/bullet/src/LinearMath/btMatrixX.h diff --git a/extern/bullet2/src/LinearMath/btMinMax.h b/extern/bullet/src/LinearMath/btMinMax.h similarity index 100% rename from extern/bullet2/src/LinearMath/btMinMax.h rename to extern/bullet/src/LinearMath/btMinMax.h diff --git a/extern/bullet2/src/LinearMath/btMotionState.h b/extern/bullet/src/LinearMath/btMotionState.h similarity index 100% rename from extern/bullet2/src/LinearMath/btMotionState.h rename to extern/bullet/src/LinearMath/btMotionState.h diff --git a/extern/bullet2/src/LinearMath/btPolarDecomposition.cpp b/extern/bullet/src/LinearMath/btPolarDecomposition.cpp similarity index 94% rename from extern/bullet2/src/LinearMath/btPolarDecomposition.cpp rename to extern/bullet/src/LinearMath/btPolarDecomposition.cpp index a4dca7fdd401..b3664faa4ea1 100644 --- a/extern/bullet2/src/LinearMath/btPolarDecomposition.cpp +++ b/extern/bullet/src/LinearMath/btPolarDecomposition.cpp @@ -30,8 +30,7 @@ namespace } } -const btScalar btPolarDecomposition::DEFAULT_TOLERANCE = btScalar(0.0001); -const unsigned int btPolarDecomposition::DEFAULT_MAX_ITERATIONS = 16; + btPolarDecomposition::btPolarDecomposition(btScalar tolerance, unsigned int maxIterations) : m_tolerance(tolerance) diff --git a/extern/bullet2/src/LinearMath/btPolarDecomposition.h b/extern/bullet/src/LinearMath/btPolarDecomposition.h similarity index 91% rename from extern/bullet2/src/LinearMath/btPolarDecomposition.h rename to extern/bullet/src/LinearMath/btPolarDecomposition.h index 561566764153..1feea0f78e85 100644 --- a/extern/bullet2/src/LinearMath/btPolarDecomposition.h +++ b/extern/bullet/src/LinearMath/btPolarDecomposition.h @@ -14,8 +14,7 @@ class btPolarDecomposition { public: - static const btScalar DEFAULT_TOLERANCE; - static const unsigned int DEFAULT_MAX_ITERATIONS; + /** * Creates an instance with optional parameters. @@ -25,8 +24,8 @@ class btPolarDecomposition * @param maxIterations - the maximum number of iterations used to achieve * convergence */ - btPolarDecomposition(btScalar tolerance = DEFAULT_TOLERANCE, - unsigned int maxIterations = DEFAULT_MAX_ITERATIONS); + btPolarDecomposition(btScalar tolerance = btScalar(0.0001), + unsigned int maxIterations = 16); /** * Decomposes a matrix into orthogonal and symmetric, positive-definite diff --git a/extern/bullet2/src/LinearMath/btPoolAllocator.h b/extern/bullet/src/LinearMath/btPoolAllocator.h similarity index 87% rename from extern/bullet2/src/LinearMath/btPoolAllocator.h rename to extern/bullet/src/LinearMath/btPoolAllocator.h index ef2084537a2f..efdeda8ffc4a 100644 --- a/extern/bullet2/src/LinearMath/btPoolAllocator.h +++ b/extern/bullet/src/LinearMath/btPoolAllocator.h @@ -18,6 +18,7 @@ subject to the following restrictions: #include "btScalar.h" #include "btAlignedAllocator.h" +#include "btThreads.h" ///The btPoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately. class btPoolAllocator @@ -27,6 +28,7 @@ class btPoolAllocator int m_freeCount; void* m_firstFree; unsigned char* m_pool; + btSpinMutex m_mutex; // only used if BT_THREADSAFE public: @@ -71,11 +73,16 @@ class btPoolAllocator { // release mode fix (void)size; + btMutexLock(&m_mutex); btAssert(!size || size<=m_elemSize); - btAssert(m_freeCount>0); + //btAssert(m_freeCount>0); // should return null if all full void* result = m_firstFree; - m_firstFree = *(void**)m_firstFree; - --m_freeCount; + if (NULL != m_firstFree) + { + m_firstFree = *(void**)m_firstFree; + --m_freeCount; + } + btMutexUnlock(&m_mutex); return result; } @@ -95,9 +102,11 @@ class btPoolAllocator if (ptr) { btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize); + btMutexLock(&m_mutex); *(void**)ptr = m_firstFree; m_firstFree = ptr; ++m_freeCount; + btMutexUnlock(&m_mutex); } } diff --git a/extern/bullet2/src/LinearMath/btQuadWord.h b/extern/bullet/src/LinearMath/btQuadWord.h similarity index 100% rename from extern/bullet2/src/LinearMath/btQuadWord.h rename to extern/bullet/src/LinearMath/btQuadWord.h diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet/src/LinearMath/btQuaternion.h similarity index 91% rename from extern/bullet2/src/LinearMath/btQuaternion.h rename to extern/bullet/src/LinearMath/btQuaternion.h index ede76938404f..c29b0be653f9 100644 --- a/extern/bullet2/src/LinearMath/btQuaternion.h +++ b/extern/bullet/src/LinearMath/btQuaternion.h @@ -141,11 +141,11 @@ class btQuaternion : public btQuadWord { * @param yaw Angle around Z * @param pitch Angle around Y * @param roll Angle around X */ - void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) + void setEulerZYX(const btScalar& yawZ, const btScalar& pitchY, const btScalar& rollX) { - btScalar halfYaw = btScalar(yaw) * btScalar(0.5); - btScalar halfPitch = btScalar(pitch) * btScalar(0.5); - btScalar halfRoll = btScalar(roll) * btScalar(0.5); + btScalar halfYaw = btScalar(yawZ) * btScalar(0.5); + btScalar halfPitch = btScalar(pitchY) * btScalar(0.5); + btScalar halfRoll = btScalar(rollX) * btScalar(0.5); btScalar cosYaw = btCos(halfYaw); btScalar sinYaw = btSin(halfYaw); btScalar cosPitch = btCos(halfPitch); @@ -157,6 +157,28 @@ class btQuaternion : public btQuadWord { cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx } + + /**@brief Get the euler angles from this quaternion + * @param yaw Angle around Z + * @param pitch Angle around Y + * @param roll Angle around X */ + void getEulerZYX(btScalar& yawZ, btScalar& pitchY, btScalar& rollX) const + { + btScalar squ; + btScalar sqx; + btScalar sqy; + btScalar sqz; + btScalar sarg; + sqx = m_floats[0] * m_floats[0]; + sqy = m_floats[1] * m_floats[1]; + sqz = m_floats[2] * m_floats[2]; + squ = m_floats[3] * m_floats[3]; + rollX = btAtan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz); + sarg = btScalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]); + pitchY = sarg <= btScalar(-1.0) ? btScalar(-0.5) * SIMD_PI: (sarg >= btScalar(1.0) ? btScalar(0.5) * SIMD_PI : btAsin(sarg)); + yawZ = btAtan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz); + } + /**@brief Add two quaternions * @param q The quaternion to add to this one */ SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q) @@ -333,7 +355,15 @@ class btQuaternion : public btQuadWord { { return btSqrt(length2()); } - + btQuaternion& safeNormalize() + { + btScalar l2 = length2(); + if (l2>SIMD_EPSILON) + { + normalize(); + } + return *this; + } /**@brief Normalize the quaternion * Such that x^2 + y^2 + z^2 +w^2 = 1 */ btQuaternion& normalize() @@ -418,22 +448,21 @@ class btQuaternion : public btQuadWord { return btAcos(dot(q) / s) * btScalar(2.0); } - /**@brief Return the angle of rotation represented by this quaternion */ + /**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */ btScalar getAngle() const { btScalar s = btScalar(2.) * btAcos(m_floats[3]); return s; } - /**@brief Return the angle of rotation represented by this quaternion along the shortest path*/ + /**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path */ btScalar getAngleShortestPath() const { btScalar s; - if (dot(*this) < 0) + if (m_floats[3] >= 0) s = btScalar(2.) * btAcos(m_floats[3]); else s = btScalar(2.) * btAcos(-m_floats[3]); - return s; } @@ -533,25 +562,29 @@ class btQuaternion : public btQuadWord { * Slerp interpolates assuming constant velocity. */ btQuaternion slerp(const btQuaternion& q, const btScalar& t) const { - btScalar magnitude = btSqrt(length2() * q.length2()); - btAssert(magnitude > btScalar(0)); - btScalar product = dot(q) / magnitude; - if (btFabs(product) < btScalar(1)) + const btScalar magnitude = btSqrt(length2() * q.length2()); + btAssert(magnitude > btScalar(0)); + + const btScalar product = dot(q) / magnitude; + const btScalar absproduct = btFabs(product); + + if(absproduct < btScalar(1.0 - SIMD_EPSILON)) { - // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp - const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1); - - const btScalar theta = btAcos(sign * product); - const btScalar s1 = btSin(sign * t * theta); - const btScalar d = btScalar(1.0) / btSin(theta); - const btScalar s0 = btSin((btScalar(1.0) - t) * theta); - - return btQuaternion( - (m_floats[0] * s0 + q.x() * s1) * d, - (m_floats[1] * s0 + q.y() * s1) * d, - (m_floats[2] * s0 + q.z() * s1) * d, - (m_floats[3] * s0 + q.m_floats[3] * s1) * d); + // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp + const btScalar theta = btAcos(absproduct); + const btScalar d = btSin(theta); + btAssert(d > btScalar(0)); + + const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1); + const btScalar s0 = btSin((btScalar(1.0) - t) * theta) / d; + const btScalar s1 = btSin(sign * t * theta) / d; + + return btQuaternion( + (m_floats[0] * s0 + q.x() * s1), + (m_floats[1] * s0 + q.y() * s1), + (m_floats[2] * s0 + q.z() * s1), + (m_floats[3] * s0 + q.w() * s1)); } else { @@ -569,7 +602,9 @@ class btQuaternion : public btQuadWord { SIMD_FORCE_INLINE void serialize(struct btQuaternionData& dataOut) const; - SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionData& dataIn); + SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionFloatData& dataIn); + + SIMD_FORCE_INLINE void deSerialize(const struct btQuaternionDoubleData& dataIn); SIMD_FORCE_INLINE void serializeFloat(struct btQuaternionFloatData& dataOut) const; @@ -970,10 +1005,16 @@ SIMD_FORCE_INLINE void btQuaternion::serialize(struct btQuaternionData& dataOut) dataOut.m_floats[i] = m_floats[i]; } -SIMD_FORCE_INLINE void btQuaternion::deSerialize(const struct btQuaternionData& dataIn) +SIMD_FORCE_INLINE void btQuaternion::deSerialize(const struct btQuaternionFloatData& dataIn) +{ + for (int i = 0; i<4; i++) + m_floats[i] = (btScalar)dataIn.m_floats[i]; +} + +SIMD_FORCE_INLINE void btQuaternion::deSerialize(const struct btQuaternionDoubleData& dataIn) { for (int i=0;i<4;i++) - m_floats[i] = dataIn.m_floats[i]; + m_floats[i] = (btScalar)dataIn.m_floats[i]; } diff --git a/extern/bullet2/src/LinearMath/btQuickprof.cpp b/extern/bullet/src/LinearMath/btQuickprof.cpp similarity index 61% rename from extern/bullet2/src/LinearMath/btQuickprof.cpp rename to extern/bullet/src/LinearMath/btQuickprof.cpp index d88d965a4cc4..d2b970152777 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.cpp +++ b/extern/bullet/src/LinearMath/btQuickprof.cpp @@ -14,11 +14,9 @@ // Ogre (www.ogre3d.org). #include "btQuickprof.h" - -#ifndef BT_NO_PROFILE +#include "btThreads.h" -static btClock gProfileClock; #ifdef __CELLOS_LV2__ @@ -30,6 +28,10 @@ static btClock gProfileClock; #if defined (SUNOS) || defined (__SUNOS__) #include #endif +#ifdef __APPLE__ +#include +#include +#endif #if defined(WIN32) || defined(_WIN32) @@ -55,6 +57,12 @@ static btClock gProfileClock; #else //_WIN32 #include + +#ifdef BT_LINUX_REALTIME +//required linking against rt (librt) +#include +#endif //BT_LINUX_REALTIME + #endif //_WIN32 #define mymin(a,b) (a > b ? a : b) @@ -65,12 +73,14 @@ struct btClockData #ifdef BT_USE_WINDOWS_TIMERS LARGE_INTEGER mClockFrequency; LONGLONG mStartTick; - LONGLONG mPrevElapsedTime; LARGE_INTEGER mStartTime; #else #ifdef __CELLOS_LV2__ uint64_t mStartTime; #else +#ifdef __APPLE__ + uint64_t mStartTimeNano; +#endif struct timeval mStartTime; #endif #endif //__CELLOS_LV2__ @@ -111,7 +121,6 @@ void btClock::reset() #ifdef BT_USE_WINDOWS_TIMERS QueryPerformanceCounter(&m_data->mStartTime); m_data->mStartTick = GetTickCount64(); - m_data->mPrevElapsedTime = 0; #else #ifdef __CELLOS_LV2__ @@ -121,6 +130,9 @@ void btClock::reset() SYS_TIMEBASE_GET( newTime ); m_data->mStartTime = newTime; #else +#ifdef __APPLE__ + m_data->mStartTimeNano = mach_absolute_time(); +#endif gettimeofday(&m_data->mStartTime, 0); #endif #endif @@ -128,7 +140,7 @@ void btClock::reset() /// Returns the time in ms since the last call to reset or since /// the btClock was created. -unsigned long int btClock::getTimeMilliseconds() +unsigned long long int btClock::getTimeMilliseconds() { #ifdef BT_USE_WINDOWS_TIMERS LARGE_INTEGER currentTime; @@ -138,27 +150,6 @@ unsigned long int btClock::getTimeMilliseconds() // Compute the number of millisecond ticks elapsed. unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / m_data->mClockFrequency.QuadPart); - // Check for unexpected leaps in the Win32 performance counter. - // (This is caused by unexpected data across the PCI to ISA - // bridge, aka south bridge. See Microsoft KB274323.) - unsigned long elapsedTicks = (unsigned long)(GetTickCount64() - m_data->mStartTick); - signed long msecOff = (signed long)(msecTicks - elapsedTicks); - if (msecOff < -100 || msecOff > 100) - { - // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - m_data->mClockFrequency.QuadPart / 1000, elapsedTime - - m_data->mPrevElapsedTime); - m_data->mStartTime.QuadPart += msecAdjustment; - elapsedTime -= msecAdjustment; - - // Recompute the number of millisecond ticks elapsed. - msecTicks = (unsigned long)(1000 * elapsedTime / - m_data->mClockFrequency.QuadPart); - } - - // Store the current elapsed time for adjustments next time. - m_data->mPrevElapsedTime = elapsedTime; return msecTicks; #else @@ -184,41 +175,19 @@ unsigned long int btClock::getTimeMilliseconds() /// Returns the time in us since the last call to reset or since /// the Clock was created. -unsigned long int btClock::getTimeMicroseconds() +unsigned long long int btClock::getTimeMicroseconds() { #ifdef BT_USE_WINDOWS_TIMERS - LARGE_INTEGER currentTime; + //see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx + LARGE_INTEGER currentTime, elapsedTime; + QueryPerformanceCounter(¤tTime); - LONGLONG elapsedTime = currentTime.QuadPart - + elapsedTime.QuadPart = currentTime.QuadPart - m_data->mStartTime.QuadPart; + elapsedTime.QuadPart *= 1000000; + elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart; - // Compute the number of millisecond ticks elapsed. - unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / - m_data->mClockFrequency.QuadPart); - - // Check for unexpected leaps in the Win32 performance counter. - // (This is caused by unexpected data across the PCI to ISA - // bridge, aka south bridge. See Microsoft KB274323.) - unsigned long elapsedTicks = (unsigned long)(GetTickCount64() - m_data->mStartTick); - signed long msecOff = (signed long)(msecTicks - elapsedTicks); - if (msecOff < -100 || msecOff > 100) - { - // Adjust the starting time forwards. - LONGLONG msecAdjustment = mymin(msecOff * - m_data->mClockFrequency.QuadPart / 1000, elapsedTime - - m_data->mPrevElapsedTime); - m_data->mStartTime.QuadPart += msecAdjustment; - elapsedTime -= msecAdjustment; - } - - // Store the current elapsed time for adjustments next time. - m_data->mPrevElapsedTime = elapsedTime; - - // Convert to microseconds. - unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / - m_data->mClockFrequency.QuadPart); - - return usecTicks; + return (unsigned long long) elapsedTime.QuadPart; #else #ifdef __CELLOS_LV2__ @@ -240,6 +209,66 @@ unsigned long int btClock::getTimeMicroseconds() #endif } +unsigned long long int btClock::getTimeNanoseconds() +{ +#ifdef BT_USE_WINDOWS_TIMERS + //see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx + LARGE_INTEGER currentTime, elapsedTime; + + QueryPerformanceCounter(¤tTime); + elapsedTime.QuadPart = currentTime.QuadPart - + m_data->mStartTime.QuadPart; + elapsedTime.QuadPart *= 1000000000; + elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart; + + return (unsigned long long) elapsedTime.QuadPart; +#else + +#ifdef __CELLOS_LV2__ + uint64_t freq=sys_time_get_timebase_frequency(); + double dFreq=((double) freq)/ 1e9; + typedef uint64_t ClockSize; + ClockSize newTime; + //__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); + SYS_TIMEBASE_GET( newTime ); + + return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq); +#else +#ifdef __APPLE__ + uint64_t ticks = mach_absolute_time() - m_data->mStartTimeNano; + static long double conversion = 0.0L; + if( 0.0L == conversion ) + { + // attempt to get conversion to nanoseconds + mach_timebase_info_data_t info; + int err = mach_timebase_info( &info ); + if( err ) + { + btAssert(0); + conversion = 1.; + } + conversion = info.numer / info.denom; + } + return (ticks * conversion); + + +#else//__APPLE__ + +#ifdef BT_LINUX_REALTIME + timespec ts; + clock_gettime(CLOCK_REALTIME,&ts); + return 1000000000*ts.tv_sec + ts.tv_nsec; +#else + struct timeval currentTime; + gettimeofday(¤tTime, 0); + return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1e9 + + (currentTime.tv_usec - m_data->mStartTime.tv_usec)*1000; +#endif //BT_LINUX_REALTIME + +#endif//__APPLE__ +#endif//__CELLOS_LV2__ +#endif +} /// Returns the time in s since the last call to reset or since @@ -250,11 +279,15 @@ btScalar btClock::getTimeSeconds() return btScalar(getTimeMicroseconds()) * microseconds_to_seconds; } +#ifndef BT_NO_PROFILE + + +static btClock gProfileClock; inline void Profile_Get_Ticks(unsigned long int * ticks) { - *ticks = gProfileClock.getTimeMicroseconds(); + *ticks = (unsigned long int)gProfileClock.getTimeMicroseconds(); } inline float Profile_Get_Tick_Rate(void) @@ -265,7 +298,6 @@ inline float Profile_Get_Tick_Rate(void) } - /*************************************************************************************************** ** ** CProfileNode @@ -367,6 +399,7 @@ 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(); } @@ -434,11 +467,71 @@ void CProfileIterator::Enter_Parent( void ) ** ***************************************************************************************************/ -CProfileNode CProfileManager::Root( "Root", NULL ); -CProfileNode * CProfileManager::CurrentNode = &CProfileManager::Root; + + + +CProfileNode gRoots[BT_QUICKPROF_MAX_THREAD_COUNT]={ + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL), + CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL) +}; + + +CProfileNode* gCurrentNodes[BT_QUICKPROF_MAX_THREAD_COUNT]= +{ + &gRoots[ 0], &gRoots[ 1], &gRoots[ 2], &gRoots[ 3], + &gRoots[ 4], &gRoots[ 5], &gRoots[ 6], &gRoots[ 7], + &gRoots[ 8], &gRoots[ 9], &gRoots[10], &gRoots[11], + &gRoots[12], &gRoots[13], &gRoots[14], &gRoots[15], + &gRoots[16], &gRoots[17], &gRoots[18], &gRoots[19], + &gRoots[20], &gRoots[21], &gRoots[22], &gRoots[23], + &gRoots[24], &gRoots[25], &gRoots[26], &gRoots[27], + &gRoots[28], &gRoots[29], &gRoots[30], &gRoots[31], + &gRoots[32], &gRoots[33], &gRoots[34], &gRoots[35], + &gRoots[36], &gRoots[37], &gRoots[38], &gRoots[39], + &gRoots[40], &gRoots[41], &gRoots[42], &gRoots[43], + &gRoots[44], &gRoots[45], &gRoots[46], &gRoots[47], + &gRoots[48], &gRoots[49], &gRoots[50], &gRoots[51], + &gRoots[52], &gRoots[53], &gRoots[54], &gRoots[55], + &gRoots[56], &gRoots[57], &gRoots[58], &gRoots[59], + &gRoots[60], &gRoots[61], &gRoots[62], &gRoots[63], +}; + + int CProfileManager::FrameCounter = 0; unsigned long int CProfileManager::ResetTime = 0; +CProfileIterator * CProfileManager::Get_Iterator( void ) +{ + + int threadIndex = btQuickprofGetCurrentThreadIndex2(); + if ((threadIndex<0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT) + return 0; + + return new CProfileIterator( &gRoots[threadIndex]); +} + +void CProfileManager::CleanupMemory(void) +{ + for (int i=0;iGet_Name()) { - CurrentNode = CurrentNode->Get_Sub_Node( name ); + int threadIndex = btQuickprofGetCurrentThreadIndex2(); + if ((threadIndex<0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT) + return; + + if (name != gCurrentNodes[threadIndex]->Get_Name()) { + gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Sub_Node( name ); } - CurrentNode->Call(); + gCurrentNodes[threadIndex]->Call(); } @@ -468,14 +565,22 @@ void CProfileManager::Start_Profile( const char * name ) *=============================================================================================*/ void CProfileManager::Stop_Profile( void ) { + int threadIndex = btQuickprofGetCurrentThreadIndex2(); + if ((threadIndex<0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT) + return; + // 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(); + if (gCurrentNodes[threadIndex]->Return()) { + gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Parent(); } } + + + + /*********************************************************************************************** * CProfileManager::Reset -- Reset the contents of the profiling system * * * @@ -484,8 +589,11 @@ void CProfileManager::Stop_Profile( void ) void CProfileManager::Reset( void ) { gProfileClock.reset(); - Root.Reset(); - Root.Call(); + int threadIndex = btQuickprofGetCurrentThreadIndex2(); + if ((threadIndex<0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT) + return; + gRoots[threadIndex].Reset(); + gRoots[threadIndex].Call(); FrameCounter = 0; Profile_Get_Ticks(&ResetTime); } @@ -572,7 +680,114 @@ void CProfileManager::dumpAll() CProfileManager::Release_Iterator(profileIterator); } +// clang-format off +#if defined(_WIN32) && (defined(__MINGW32__) || defined(__MINGW64__)) + #define BT_HAVE_TLS 1 +#elif __APPLE__ && !TARGET_OS_IPHONE + // TODO: Modern versions of iOS support TLS now with updated version checking. + #define BT_HAVE_TLS 1 +#elif __linux__ + #define BT_HAVE_TLS 1 +#endif + +// __thread is broken on Andorid clang until r12b. See +// https://github.com/android-ndk/ndk/issues/8 +#if defined(__ANDROID__) && defined(__clang__) + #if __has_include() + #include + #endif // __has_include() + #if defined(__NDK_MAJOR__) && \ + ((__NDK_MAJOR__ < 12) || ((__NDK_MAJOR__ == 12) && (__NDK_MINOR__ < 1))) + #undef BT_HAVE_TLS + #endif +#endif // defined(__ANDROID__) && defined(__clang__) +// clang-format on + +unsigned int btQuickprofGetCurrentThreadIndex2() { + const unsigned int kNullIndex = ~0U; + +#if BT_THREADSAFE + return btGetCurrentThreadIndex(); +#else +#if defined(BT_HAVE_TLS) + static __thread unsigned int sThreadIndex = kNullIndex; +#elif defined(_WIN32) + __declspec(thread) static unsigned int sThreadIndex = kNullIndex; +#else + unsigned int sThreadIndex = 0; + return -1; +#endif + + static int gThreadCounter = 0; + + if (sThreadIndex == kNullIndex) { + sThreadIndex = gThreadCounter++; + } + return sThreadIndex; +#endif //BT_THREADSAFE +} +void btEnterProfileZoneDefault(const char* name) +{ + CProfileManager::Start_Profile( name ); +} +void btLeaveProfileZoneDefault() +{ + CProfileManager::Stop_Profile(); +} +#else +void btEnterProfileZoneDefault(const char* name) +{ +} +void btLeaveProfileZoneDefault() +{ +} #endif //BT_NO_PROFILE + + + + + +static btEnterProfileZoneFunc* bts_enterFunc = btEnterProfileZoneDefault; +static btLeaveProfileZoneFunc* bts_leaveFunc = btLeaveProfileZoneDefault; + +void btEnterProfileZone(const char* name) +{ + (bts_enterFunc)(name); +} +void btLeaveProfileZone() +{ + (bts_leaveFunc)(); +} + +btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc() +{ + return bts_enterFunc ; +} +btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc() +{ + return bts_leaveFunc; +} + + +void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc) +{ + bts_enterFunc = enterFunc; +} +void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc) +{ + bts_leaveFunc = leaveFunc; +} + +CProfileSample::CProfileSample( const char * name ) +{ + btEnterProfileZone(name); +} + +CProfileSample::~CProfileSample( void ) +{ + btLeaveProfileZone(); +} + diff --git a/extern/bullet2/src/LinearMath/btQuickprof.h b/extern/bullet/src/LinearMath/btQuickprof.h similarity index 81% rename from extern/bullet2/src/LinearMath/btQuickprof.h rename to extern/bullet/src/LinearMath/btQuickprof.h index 362f62d6d400..7b38d71b90db 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.h +++ b/extern/bullet/src/LinearMath/btQuickprof.h @@ -15,18 +15,7 @@ #ifndef BT_QUICK_PROF_H #define BT_QUICK_PROF_H -//To disable built-in profiling, please comment out next line -//#define BT_NO_PROFILE 1 -#ifndef BT_NO_PROFILE -#include //@todo remove this, backwards compatibility #include "btScalar.h" -#include "btAlignedAllocator.h" -#include - - - - - #define USE_BT_CLOCK 1 #ifdef USE_BT_CLOCK @@ -47,12 +36,14 @@ class btClock /// Returns the time in ms since the last call to reset or since /// the btClock was created. - unsigned long int getTimeMilliseconds(); + unsigned long long int getTimeMilliseconds(); /// Returns the time in us since the last call to reset or since /// the Clock was created. - unsigned long int getTimeMicroseconds(); + unsigned long long int getTimeMicroseconds(); + unsigned long long int getTimeNanoseconds(); + /// Returns the time in s since the last call to reset or since /// the Clock was created. btScalar getTimeSeconds(); @@ -63,6 +54,38 @@ class btClock #endif //USE_BT_CLOCK +typedef void (btEnterProfileZoneFunc)(const char* msg); +typedef void (btLeaveProfileZoneFunc)(); + +btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc(); +btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc(); + + + +void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc); +void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc); + +#ifndef BT_NO_PROFILE // FIX redefinition +//To disable built-in profiling, please comment out next line +//#define BT_NO_PROFILE 1 +#endif //BT_NO_PROFILE + +#ifndef BT_NO_PROFILE +//btQuickprofGetCurrentThreadIndex will return -1 if thread index cannot be determined, +//otherwise returns thread index in range [0..maxThreads] +unsigned int btQuickprofGetCurrentThreadIndex2(); +const unsigned int BT_QUICKPROF_MAX_THREAD_COUNT = 64; + +#include //@todo remove this, backwards compatibility + +#include "btAlignedAllocator.h" +#include + + + + + + @@ -148,21 +171,21 @@ class CProfileManager { static void Start_Profile( const char * name ); static void Stop_Profile( void ); - static void CleanupMemory(void) - { - Root.CleanupMemory(); - } + static void CleanupMemory(void); +// { +// Root.CleanupMemory(); +// } static void Reset( void ); static void Increment_Frame_Counter( void ); static int Get_Frame_Count_Since_Reset( void ) { return FrameCounter; } static float Get_Time_Since_Reset( void ); - static CProfileIterator * Get_Iterator( void ) - { - - return new CProfileIterator( &Root ); - } + static CProfileIterator * Get_Iterator( void ); +// { +// +// return new CProfileIterator( &Root ); +// } static void Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); } static void dumpRecursive(CProfileIterator* profileIterator, int spacing); @@ -170,37 +193,27 @@ class CProfileManager { static void dumpAll(); private: - static CProfileNode Root; - static CProfileNode * CurrentNode; + static int FrameCounter; static unsigned long int ResetTime; }; + + +#endif //#ifndef BT_NO_PROFILE + ///ProfileSampleClass is a simple way to profile a function's scope ///Use the BT_PROFILE macro at the start of scope to time class CProfileSample { public: - CProfileSample( const char * name ) - { - CProfileManager::Start_Profile( name ); - } - - ~CProfileSample( void ) - { - CProfileManager::Stop_Profile(); - } -}; + CProfileSample( const char * name ); + ~CProfileSample( void ); +}; #define BT_PROFILE( name ) CProfileSample __profile( name ) -#else - -#define BT_PROFILE( name ) - -#endif //#ifndef BT_NO_PROFILE - #endif //BT_QUICK_PROF_H diff --git a/extern/bullet2/src/LinearMath/btRandom.h b/extern/bullet/src/LinearMath/btRandom.h similarity index 100% rename from extern/bullet2/src/LinearMath/btRandom.h rename to extern/bullet/src/LinearMath/btRandom.h diff --git a/extern/bullet/src/LinearMath/btScalar.h b/extern/bullet/src/LinearMath/btScalar.h new file mode 100644 index 000000000000..2072b014cdd1 --- /dev/null +++ b/extern/bullet/src/LinearMath/btScalar.h @@ -0,0 +1,813 @@ +/* +Copyright (c) 2003-2009 Erwin Coumans http://bullet.googlecode.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SCALAR_H +#define BT_SCALAR_H +#if defined(_MSC_VER) && defined(__clang__) /* clang supplies it's own overloads already */ +#define BT_NO_SIMD_OPERATOR_OVERLOADS +#endif + +#ifdef BT_MANAGED_CODE +//Aligned data types not supported in managed code +#pragma unmanaged +#endif + +#include +#include //size_t for MSVC 6.0 +#include + +/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ +#define BT_BULLET_VERSION 288 + +inline int btGetVersion() +{ + return BT_BULLET_VERSION; +} + + +// The following macro "BT_NOT_EMPTY_FILE" can be put into a file +// in order suppress the MS Visual C++ Linker warning 4221 +// +// warning LNK4221: no public symbols found; archive member will be inaccessible +// +// This warning occurs on PC and XBOX when a file compiles out completely +// has no externally visible symbols which may be dependant on configuration +// #defines and options. +// +// see more https://stackoverflow.com/questions/1822887/what-is-the-best-way-to-eliminate-ms-visual-c-linker-warning-warning-lnk422 + +#if defined (_MSC_VER) + #define BT_NOT_EMPTY_FILE_CAT_II(p, res) res + #define BT_NOT_EMPTY_FILE_CAT_I(a, b) BT_NOT_EMPTY_FILE_CAT_II(~, a ## b) + #define BT_NOT_EMPTY_FILE_CAT(a, b) BT_NOT_EMPTY_FILE_CAT_I(a, b) + #define BT_NOT_EMPTY_FILE namespace { char BT_NOT_EMPTY_FILE_CAT(NoEmptyFileDummy, __COUNTER__); } +#else + #define BT_NOT_EMPTY_FILE +#endif + + +// clang and most formatting tools don't support indentation of preprocessor guards, so turn it off +// clang-format off +#if defined(DEBUG) || defined (_DEBUG) + #define BT_DEBUG +#endif + +#ifdef _WIN32 + #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) + #define SIMD_FORCE_INLINE inline + #define ATTRIBUTE_ALIGNED16(a) a + #define ATTRIBUTE_ALIGNED64(a) a + #define ATTRIBUTE_ALIGNED128(a) a + #elif defined(_M_ARM) + #define SIMD_FORCE_INLINE __forceinline + #define ATTRIBUTE_ALIGNED16(a) __declspec() a + #define ATTRIBUTE_ALIGNED64(a) __declspec() a + #define ATTRIBUTE_ALIGNED128(a) __declspec () a + #else//__MINGW32__ + //#define BT_HAS_ALIGNED_ALLOCATOR + #pragma warning(disable : 4324) // disable padding warning +// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. + #pragma warning(disable:4996) //Turn off warnings about deprecated C routines +// #pragma warning(disable:4786) // Disable the "debug name too long" warning + + #define SIMD_FORCE_INLINE __forceinline + #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a + #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a + #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a + #ifdef _XBOX + #define BT_USE_VMX128 + + #include + #define BT_HAVE_NATIVE_FSEL + #define btFsel(a,b,c) __fsel((a),(b),(c)) + #else + +#if defined (_M_ARM) + //Do not turn SSE on for ARM (may want to turn on BT_USE_NEON however) +#elif (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION)) + #if _MSC_VER>1400 + #define BT_USE_SIMD_VECTOR3 + #endif + + #define BT_USE_SSE + #ifdef BT_USE_SSE + +#if (_MSC_FULL_VER >= 170050727)//Visual Studio 2012 can compile SSE4/FMA3 (but SSE4/FMA3 is not enabled by default) + //#define BT_ALLOW_SSE4 //disable this cause blender targets sse2 +#endif //(_MSC_FULL_VER >= 160040219) + + //BT_USE_SSE_IN_API is disabled under Windows by default, because + //it makes it harder to integrate Bullet into your application under Windows + //(structured embedding Bullet structs/classes need to be 16-byte aligned) + //with relatively little performance gain + //If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries + //you can manually enable this line or set it in the build system for a bit of performance gain (a few percent, dependent on usage) + //#define BT_USE_SSE_IN_API + #endif //BT_USE_SSE + #include +#endif + + #endif//_XBOX + + #endif //__MINGW32__ + + #ifdef BT_DEBUG + #if defined(_MSC_VER) && !defined(__clang__) + #include + #define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u (%s)\n", __LINE__, #x);__debugbreak(); }} + #else//_MSC_VER + #include + #define btAssert assert + #endif//_MSC_VER + #else + #define btAssert(x) + #endif + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) + + #define btLikely(_c) _c + #define btUnlikely(_c) _c + +#else//_WIN32 + + #if defined (__CELLOS_LV2__) + #define SIMD_FORCE_INLINE inline __attribute__((always_inline)) + #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #ifndef assert + #include + #endif + #ifdef BT_DEBUG + #ifdef __SPU__ + #include + #define printf spu_printf + #define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}} + #else + #define btAssert assert + #endif + + #else//BT_DEBUG + #define btAssert(x) + #endif//BT_DEBUG + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) + + #define btLikely(_c) _c + #define btUnlikely(_c) _c + + #else//defined (__CELLOS_LV2__) + + #ifdef USE_LIBSPE2 + + #define SIMD_FORCE_INLINE __inline + #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #ifndef assert + #include + #endif + #ifdef BT_DEBUG + #define btAssert assert + #else + #define btAssert(x) + #endif + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) + + + #define btLikely(_c) __builtin_expect((_c), 1) + #define btUnlikely(_c) __builtin_expect((_c), 0) + + + #else//USE_LIBSPE2 + //non-windows systems + + #if (defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION))) + #if defined (__i386__) || defined (__x86_64__) + #define BT_USE_SIMD_VECTOR3 + #define BT_USE_SSE + //BT_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries + //if apps run into issues, we will disable the next line + #define BT_USE_SSE_IN_API + #ifdef BT_USE_SSE + // include appropriate SSE level + #if defined (__SSE4_1__) + #include + #elif defined (__SSSE3__) + #include + #elif defined (__SSE3__) + #include + #else + #include + #endif + #endif //BT_USE_SSE + #elif defined( __ARM_NEON__ ) + #ifdef __clang__ + #define BT_USE_NEON 1 + #define BT_USE_SIMD_VECTOR3 + + #if defined BT_USE_NEON && defined (__clang__) + #include + #endif//BT_USE_NEON + #endif //__clang__ + #endif//__arm__ + + #define SIMD_FORCE_INLINE inline __attribute__ ((always_inline)) + ///@todo: check out alignment methods for other platforms/compilers + #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #ifndef assert + #include + #endif + + #if defined(DEBUG) || defined (_DEBUG) + #if defined (__i386__) || defined (__x86_64__) + #include + #define btAssert(x)\ + {\ + if(!(x))\ + {\ + printf("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\ + asm volatile ("int3");\ + }\ + } + #else//defined (__i386__) || defined (__x86_64__) + #define btAssert assert + #endif//defined (__i386__) || defined (__x86_64__) + #else//defined(DEBUG) || defined (_DEBUG) + #define btAssert(x) + #endif//defined(DEBUG) || defined (_DEBUG) + + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) + #define btLikely(_c) _c + #define btUnlikely(_c) _c + + #else//__APPLE__ + + #define SIMD_FORCE_INLINE inline + ///@todo: check out alignment methods for other platforms/compilers + ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) + ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) + #define ATTRIBUTE_ALIGNED16(a) a + #define ATTRIBUTE_ALIGNED64(a) a + #define ATTRIBUTE_ALIGNED128(a) a + #ifndef assert + #include + #endif + + #if defined(DEBUG) || defined (_DEBUG) + #define btAssert assert + #else + #define btAssert(x) + #endif + + //btFullAssert is optional, slows down a lot + #define btFullAssert(x) + #define btLikely(_c) _c + #define btUnlikely(_c) _c + #endif //__APPLE__ + #endif // LIBSPE2 + #endif //__CELLOS_LV2__ +#endif//_WIN32 + + +///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision. +#if defined(BT_USE_DOUBLE_PRECISION) + typedef double btScalar; + //this number could be bigger in double precision + #define BT_LARGE_FLOAT 1e30 +#else + typedef float btScalar; + //keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX + #define BT_LARGE_FLOAT 1e18f +#endif + +#ifdef BT_USE_SSE + typedef __m128 btSimdFloat4; +#endif //BT_USE_SSE + +#if defined(BT_USE_SSE) + //#if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE) + #ifdef _WIN32 + + #ifndef BT_NAN + static int btNanMask = 0x7F800001; + #define BT_NAN (*(float *)&btNanMask) + #endif + + #ifndef BT_INFINITY + static int btInfinityMask = 0x7F800000; + #define BT_INFINITY (*(float *)&btInfinityMask) + inline int btGetInfinityMask() //suppress stupid compiler warning + { + return btInfinityMask; + } + #endif + + + + //use this, in case there are clashes (such as xnamath.h) + #ifndef BT_NO_SIMD_OPERATOR_OVERLOADS + inline __m128 operator+(const __m128 A, const __m128 B) + { + return _mm_add_ps(A, B); + } + + inline __m128 operator-(const __m128 A, const __m128 B) + { + return _mm_sub_ps(A, B); + } + + inline __m128 operator*(const __m128 A, const __m128 B) + { + return _mm_mul_ps(A, B); + } + #endif //BT_NO_SIMD_OPERATOR_OVERLOADS + + #define btCastfTo128i(a) (_mm_castps_si128(a)) + #define btCastfTo128d(a) (_mm_castps_pd(a)) + #define btCastiTo128f(a) (_mm_castsi128_ps(a)) + #define btCastdTo128f(a) (_mm_castpd_ps(a)) + #define btCastdTo128i(a) (_mm_castpd_si128(a)) + #define btAssign128(r0, r1, r2, r3) _mm_setr_ps(r0, r1, r2, r3) + + #else //_WIN32 + + #define btCastfTo128i(a) ((__m128i)(a)) + #define btCastfTo128d(a) ((__m128d)(a)) + #define btCastiTo128f(a) ((__m128)(a)) + #define btCastdTo128f(a) ((__m128)(a)) + #define btCastdTo128i(a) ((__m128i)(a)) + #define btAssign128(r0, r1, r2, r3) \ + (__m128) { r0, r1, r2, r3 } + #define BT_INFINITY INFINITY + #define BT_NAN NAN + #endif //_WIN32 +#else//BT_USE_SSE + + #ifdef BT_USE_NEON + #include + + typedef float32x4_t btSimdFloat4; + #define BT_INFINITY INFINITY + #define BT_NAN NAN + #define btAssign128(r0, r1, r2, r3) \ + (float32x4_t) { r0, r1, r2, r3 } + #else //BT_USE_NEON + + #ifndef BT_INFINITY + struct btInfMaskConverter + { + union { + float mask; + int intmask; + }; + btInfMaskConverter(int _mask = 0x7F800000) + : intmask(_mask) + { + } + }; + static btInfMaskConverter btInfinityMask = 0x7F800000; + #define BT_INFINITY (btInfinityMask.mask) + inline int btGetInfinityMask() //suppress stupid compiler warning + { + return btInfinityMask.intmask; + } + #endif + #endif //BT_USE_NEON + +#endif //BT_USE_SSE + +#ifdef BT_USE_NEON + #include + + typedef float32x4_t btSimdFloat4; + #define BT_INFINITY INFINITY + #define BT_NAN NAN + #define btAssign128(r0, r1, r2, r3) \ + (float32x4_t) { r0, r1, r2, r3 } +#endif//BT_USE_NEON + +#define BT_DECLARE_ALIGNED_ALLOCATOR() \ + SIMD_FORCE_INLINE void *operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); } \ + SIMD_FORCE_INLINE void operator delete(void *ptr) { btAlignedFree(ptr); } \ + SIMD_FORCE_INLINE void *operator new(size_t, void *ptr) { return ptr; } \ + SIMD_FORCE_INLINE void operator delete(void *, void *) {} \ + SIMD_FORCE_INLINE void *operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes, 16); } \ + SIMD_FORCE_INLINE void operator delete[](void *ptr) { btAlignedFree(ptr); } \ + SIMD_FORCE_INLINE void *operator new[](size_t, void *ptr) { return ptr; } \ + SIMD_FORCE_INLINE void operator delete[](void *, void *) {} + +#if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS) + + SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) + { + return sqrt(x); + } + SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); } + SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); } + SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); } + SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); } + SIMD_FORCE_INLINE btScalar btAcos(btScalar x) + { + if (x < btScalar(-1)) x = btScalar(-1); + if (x > btScalar(1)) x = btScalar(1); + return acos(x); + } + SIMD_FORCE_INLINE btScalar btAsin(btScalar x) + { + if (x < btScalar(-1)) x = btScalar(-1); + if (x > btScalar(1)) x = btScalar(1); + return asin(x); + } + SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); } + SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); } + SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); } + SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); } + SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) { return pow(x, y); } + SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) { return fmod(x, y); } + +#else//BT_USE_DOUBLE_PRECISION + + SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) + { + #ifdef USE_APPROXIMATION + #ifdef __LP64__ + float xhalf = 0.5f * y; + int i = *(int *)&y; + i = 0x5f375a86 - (i >> 1); + y = *(float *)&i; + y = y * (1.5f - xhalf * y * y); + y = y * (1.5f - xhalf * y * y); + y = y * (1.5f - xhalf * y * y); + y = 1 / y; + return y; + #else + double x, z, tempf; + unsigned long *tfptr = ((unsigned long *)&tempf) + 1; + tempf = y; + *tfptr = (0xbfcdd90a - *tfptr) >> 1; /* estimate of 1/sqrt(y) */ + x = tempf; + z = y * btScalar(0.5); + x = (btScalar(1.5) * x) - (x * x) * (x * z); /* iteration formula */ + x = (btScalar(1.5) * x) - (x * x) * (x * z); + x = (btScalar(1.5) * x) - (x * x) * (x * z); + x = (btScalar(1.5) * x) - (x * x) * (x * z); + x = (btScalar(1.5) * x) - (x * x) * (x * z); + return x * y; + #endif + #else + return sqrtf(y); + #endif + } + SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); } + SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); } + SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); } + SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); } + SIMD_FORCE_INLINE btScalar btAcos(btScalar x) + { + if (x < btScalar(-1)) + x = btScalar(-1); + if (x > btScalar(1)) + x = btScalar(1); + return acosf(x); + } + SIMD_FORCE_INLINE btScalar btAsin(btScalar x) + { + if (x < btScalar(-1)) + x = btScalar(-1); + if (x > btScalar(1)) + x = btScalar(1); + return asinf(x); + } + SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } + SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } + SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); } + SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); } + SIMD_FORCE_INLINE btScalar btPow(btScalar x, btScalar y) { return powf(x, y); } + SIMD_FORCE_INLINE btScalar btFmod(btScalar x, btScalar y) { return fmodf(x, y); } + +#endif//BT_USE_DOUBLE_PRECISION + +#define SIMD_PI btScalar(3.1415926535897932384626433832795029) +#define SIMD_2_PI (btScalar(2.0) * SIMD_PI) +#define SIMD_HALF_PI (SIMD_PI * btScalar(0.5)) +#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0)) +#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI) +#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) +#define btRecipSqrt(x) ((btScalar)(btScalar(1.0) / btSqrt(btScalar(x)))) /* reciprocal square root */ +#define btRecip(x) (btScalar(1.0) / btScalar(x)) + +#ifdef BT_USE_DOUBLE_PRECISION + #define SIMD_EPSILON DBL_EPSILON + #define SIMD_INFINITY DBL_MAX + #define BT_ONE 1.0 + #define BT_ZERO 0.0 + #define BT_TWO 2.0 + #define BT_HALF 0.5 +#else + #define SIMD_EPSILON FLT_EPSILON + #define SIMD_INFINITY FLT_MAX + #define BT_ONE 1.0f + #define BT_ZERO 0.0f + #define BT_TWO 2.0f + #define BT_HALF 0.5f +#endif + +// clang-format on + +SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) +{ + btScalar coeff_1 = SIMD_PI / 4.0f; + btScalar coeff_2 = 3.0f * coeff_1; + btScalar abs_y = btFabs(y); + btScalar angle; + if (x >= 0.0f) + { + btScalar r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } + else + { + btScalar r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + return (y < 0.0f) ? -angle : angle; +} + +SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; } + +SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) +{ + return (((a) <= eps) && !((a) < -eps)); +} +SIMD_FORCE_INLINE bool btGreaterEqual(btScalar a, btScalar eps) +{ + return (!((a) <= eps)); +} + +SIMD_FORCE_INLINE int btIsNegative(btScalar x) +{ + return x < btScalar(0.0) ? 1 : 0; +} + +SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; } +SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; } + +#define BT_DECLARE_HANDLE(name) \ + typedef struct name##__ \ + { \ + int unused; \ + } * name + +#ifndef btFsel +SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c) +{ + return a >= 0 ? b : c; +} +#endif +#define btFsels(a, b, c) (btScalar) btFsel(a, b, c) + +SIMD_FORCE_INLINE bool btMachineIsLittleEndian() +{ + long int i = 1; + const char *p = (const char *)&i; + if (p[0] == 1) // Lowest address contains the least significant byte + return true; + else + return false; +} + +///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360 +///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html +SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) +{ + // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero + // Rely on positive value or'ed with its negative having sign bit on + // and zero value or'ed with its negative (which is still zero) having sign bit off + // Use arithmetic shift right, shifting the sign bit through all 32 bits + unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); + unsigned testEqz = ~testNz; + return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); +} +SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero) +{ + unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); + unsigned testEqz = ~testNz; + return static_cast((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); +} +SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) +{ +#ifdef BT_HAVE_NATIVE_FSEL + return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); +#else + return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; +#endif +} + +template +SIMD_FORCE_INLINE void btSwap(T &a, T &b) +{ + T tmp = a; + a = b; + b = tmp; +} + +//PCK: endian swapping functions +SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val) +{ + return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24)); +} + +SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val) +{ + return static_cast(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8)); +} + +SIMD_FORCE_INLINE unsigned btSwapEndian(int val) +{ + return btSwapEndian((unsigned)val); +} + +SIMD_FORCE_INLINE unsigned short btSwapEndian(short val) +{ + return btSwapEndian((unsigned short)val); +} + +///btSwapFloat uses using char pointers to swap the endianness +////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values +///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. +///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. +///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. +///so instead of returning a float/double, we return integer/long long integer +SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d) +{ + unsigned int a = 0; + unsigned char *dst = (unsigned char *)&a; + unsigned char *src = (unsigned char *)&d; + + dst[0] = src[3]; + dst[1] = src[2]; + dst[2] = src[1]; + dst[3] = src[0]; + return a; +} + +// unswap using char pointers +SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a) +{ + float d = 0.0f; + unsigned char *src = (unsigned char *)&a; + unsigned char *dst = (unsigned char *)&d; + + dst[0] = src[3]; + dst[1] = src[2]; + dst[2] = src[1]; + dst[3] = src[0]; + + return d; +} + +// swap using char pointers +SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char *dst) +{ + unsigned char *src = (unsigned char *)&d; + + dst[0] = src[7]; + dst[1] = src[6]; + dst[2] = src[5]; + dst[3] = src[4]; + dst[4] = src[3]; + dst[5] = src[2]; + dst[6] = src[1]; + dst[7] = src[0]; +} + +// unswap using char pointers +SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) +{ + double d = 0.0; + unsigned char *dst = (unsigned char *)&d; + + dst[0] = src[7]; + dst[1] = src[6]; + dst[2] = src[5]; + dst[3] = src[4]; + dst[4] = src[3]; + dst[5] = src[2]; + dst[6] = src[1]; + dst[7] = src[0]; + + return d; +} + +template +SIMD_FORCE_INLINE void btSetZero(T *a, int n) +{ + T *acurr = a; + size_t ncurr = n; + while (ncurr > 0) + { + *(acurr++) = 0; + --ncurr; + } +} + +SIMD_FORCE_INLINE btScalar btLargeDot(const btScalar *a, const btScalar *b, int n) +{ + btScalar p0, q0, m0, p1, q1, m1, sum; + sum = 0; + n -= 2; + while (n >= 0) + { + p0 = a[0]; + q0 = b[0]; + m0 = p0 * q0; + p1 = a[1]; + q1 = b[1]; + m1 = p1 * q1; + sum += m0; + sum += m1; + a += 2; + b += 2; + n -= 2; + } + n += 2; + while (n > 0) + { + sum += (*a) * (*b); + a++; + b++; + n--; + } + return sum; +} + +// returns normalized value in range [-SIMD_PI, SIMD_PI] +SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) +{ + angleInRadians = btFmod(angleInRadians, SIMD_2_PI); + if (angleInRadians < -SIMD_PI) + { + return angleInRadians + SIMD_2_PI; + } + else if (angleInRadians > SIMD_PI) + { + return angleInRadians - SIMD_2_PI; + } + else + { + return angleInRadians; + } +} + +///rudimentary class to provide type info +struct btTypedObject +{ + btTypedObject(int objectType) + : m_objectType(objectType) + { + } + int m_objectType; + inline int getObjectType() const + { + return m_objectType; + } +}; + +///align a pointer to the provided alignment, upwards +template +T *btAlignPointer(T *unalignedPtr, size_t alignment) +{ + struct btConvertPointerSizeT + { + union { + T *ptr; + size_t integer; + }; + }; + btConvertPointerSizeT converter; + + const size_t bit_mask = ~(alignment - 1); + converter.ptr = unalignedPtr; + converter.integer += alignment - 1; + converter.integer &= bit_mask; + return converter.ptr; +} + +#endif //BT_SCALAR_H diff --git a/extern/bullet/src/LinearMath/btSerializer.cpp b/extern/bullet/src/LinearMath/btSerializer.cpp new file mode 100644 index 000000000000..4faa8f536bfa --- /dev/null +++ b/extern/bullet/src/LinearMath/btSerializer.cpp @@ -0,0 +1,689 @@ +char sBulletDNAstr[]= { +char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(-76),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), +char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95), +char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111), +char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(115),char(0),char(42),char(102),char(105),char(114),char(115),char(116),char(0),char(42),char(108),char(97),char(115), +char(116),char(0),char(109),char(95),char(102),char(108),char(111),char(97),char(116),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(108),char(91),char(51), +char(93),char(0),char(109),char(95),char(98),char(97),char(115),char(105),char(115),char(0),char(109),char(95),char(111),char(114),char(105),char(103),char(105),char(110),char(0),char(109), +char(95),char(114),char(111),char(111),char(116),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98), +char(116),char(114),char(101),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100), +char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(91),char(51),char(93),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122), +char(101),char(100),char(65),char(97),char(98),char(98),char(77),char(97),char(120),char(91),char(51),char(93),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77), +char(105),char(110),char(79),char(114),char(103),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77),char(97),char(120),char(79),char(114),char(103),char(0),char(109), +char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98),char(80),char(97), +char(114),char(116),char(0),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109), +char(95),char(112),char(97),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101), +char(120),char(79),char(114),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(98), +char(118),char(104),char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(0),char(109),char(95),char(98),char(118),char(104),char(65),char(97),char(98),char(98),char(77), +char(97),char(120),char(0),char(109),char(95),char(98),char(118),char(104),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110), +char(0),char(109),char(95),char(99),char(117),char(114),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(117),char(115), +char(101),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(110),char(117),char(109),char(67), +char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(76),char(101),char(97),char(102),char(78),char(111),char(100),char(101),char(115),char(0),char(109), +char(95),char(110),char(117),char(109),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117), +char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(99),char(111),char(110),char(116),char(105),char(103),char(117),char(111), +char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105), +char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116), +char(114),char(0),char(42),char(109),char(95),char(115),char(117),char(98),char(84),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(80),char(116),char(114),char(0), +char(109),char(95),char(116),char(114),char(97),char(118),char(101),char(114),char(115),char(97),char(108),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(110),char(117), +char(109),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(72),char(101),char(97),char(100),char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(110), +char(97),char(109),char(101),char(0),char(109),char(95),char(115),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(112),char(97), +char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110), +char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(83),char(99),char(97), +char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(101),char(78),char(111),char(114),char(109),char(97),char(108),char(0),char(109), +char(95),char(112),char(108),char(97),char(110),char(101),char(67),char(111),char(110),char(115),char(116),char(97),char(110),char(116),char(0),char(109),char(95),char(105),char(109),char(112), +char(108),char(105),char(99),char(105),char(116),char(83),char(104),char(97),char(112),char(101),char(68),char(105),char(109),char(101),char(110),char(115),char(105),char(111),char(110),char(115), +char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(109), +char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(111),char(115),char(0),char(109),char(95),char(114),char(97),char(100), +char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108), +char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(42),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(80),char(111), +char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(108),char(111),char(99), +char(97),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(83),char(105),char(122),char(101),char(0), +char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(0),char(109),char(95),char(112),char(97),char(100),char(91),char(50),char(93),char(0),char(109),char(95),char(118), +char(97),char(108),char(117),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(112),char(97),char(100),char(0),char(42),char(109),char(95),char(118),char(101), +char(114),char(116),char(105),char(99),char(101),char(115),char(51),char(102),char(0),char(42),char(109),char(95),char(118),char(101),char(114),char(116),char(105),char(99),char(101),char(115), +char(51),char(100),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(51),char(50),char(0),char(42),char(109),char(95),char(51), +char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(42),char(109),char(95),char(51),char(105),char(110),char(100),char(105),char(99),char(101), +char(115),char(56),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(109),char(95),char(110),char(117), +char(109),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(101),char(114),char(116), +char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(115),char(80),char(116),char(114), +char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(101),char(115),char(104), +char(80),char(97),char(114),char(116),char(115),char(0),char(109),char(95),char(109),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99), +char(101),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(70),char(108),char(111),char(97),char(116),char(66), +char(118),char(104),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(68),char(111),char(117),char(98),char(108), +char(101),char(66),char(118),char(104),char(0),char(42),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111), +char(77),char(97),char(112),char(0),char(109),char(95),char(112),char(97),char(100),char(51),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(114),char(105),char(109), +char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(116),char(114),char(97),char(110),char(115), +char(102),char(111),char(114),char(109),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(0),char(109), +char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104), +char(105),char(108),char(100),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104), +char(97),char(112),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(104),char(105),char(108),char(100),char(83),char(104),char(97), +char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(117),char(112),char(73),char(110),char(100), +char(101),char(120),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86), +char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103), +char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42), +char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101), +char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80), +char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95), +char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110), +char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114), +char(116),char(101),char(120),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(68), +char(105),char(115),char(116),char(97),char(110),char(99),char(101),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(122), +char(101),char(114),char(111),char(65),char(114),char(101),char(97),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110), +char(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83), +char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117), +char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(76),char(111), +char(99),char(97),char(108),char(80),char(111),char(105),char(110),char(116),char(65),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116), +char(67),char(97),char(99),char(104),char(101),char(76),char(111),char(99),char(97),char(108),char(80),char(111),char(105),char(110),char(116),char(66),char(91),char(52),char(93),char(0), +char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110), +char(87),char(111),char(114),char(108),char(100),char(79),char(110),char(65),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67), +char(97),char(99),char(104),char(101),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(79),char(110),char(66), +char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(78),char(111),char(114),char(109), +char(97),char(108),char(87),char(111),char(114),char(108),char(100),char(79),char(110),char(66),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110), +char(116),char(67),char(97),char(99),char(104),char(101),char(76),char(97),char(116),char(101),char(114),char(97),char(108),char(70),char(114),char(105),char(99),char(116),char(105),char(111), +char(110),char(68),char(105),char(114),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104), +char(101),char(76),char(97),char(116),char(101),char(114),char(97),char(108),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(68),char(105),char(114),char(50), +char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(68),char(105),char(115),char(116), +char(97),char(110),char(99),char(101),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101), +char(65),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(91),char(52),char(93),char(0),char(109),char(95), +char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(70),char(114), +char(105),char(99),char(116),char(105),char(111),char(110),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99), +char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105), +char(99),char(116),char(105),char(111),char(110),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104), +char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(83),char(112),char(105),char(110),char(110),char(105),char(110),char(103),char(70),char(114),char(105), +char(99),char(116),char(105),char(111),char(110),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104), +char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110), +char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(80),char(97),char(114),char(116), +char(73),char(100),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(80), +char(97),char(114),char(116),char(73),char(100),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99), +char(104),char(101),char(73),char(110),char(100),char(101),char(120),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67), +char(97),char(99),char(104),char(101),char(73),char(110),char(100),char(101),char(120),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110), +char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(111),char(105),char(110),char(116),char(70),char(108), +char(97),char(103),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(65), +char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(76),char(97),char(116),char(101),char(114),char(97),char(108), +char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(65),char(112),char(112), +char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(76),char(97),char(116),char(101),char(114),char(97),char(108),char(50),char(91), +char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97), +char(99),char(116),char(77),char(111),char(116),char(105),char(111),char(110),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116), +char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(77),char(111),char(116),char(105),char(111),char(110),char(50),char(91), +char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97), +char(99),char(116),char(67),char(70),char(77),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104), +char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(116),char(105),char(102), +char(102),char(110),char(101),char(115),char(115),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99), +char(104),char(101),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(69),char(82),char(80),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111), +char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(67),char(111),char(110),char(116), +char(97),char(99),char(116),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105), +char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(67),char(70),char(77),char(91),char(52), +char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(76),char(105),char(102),char(101),char(84),char(105), +char(109),char(101),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(97),char(99),char(104),char(101),char(100),char(80),char(111),char(105), +char(110),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(65),char(0),char(109), +char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(66),char(0),char(109),char(95),char(105),char(110),char(100),char(101), +char(120),char(49),char(97),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99), +char(111),char(110),char(116),char(97),char(99),char(116),char(66),char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104), +char(111),char(108),char(100),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(114),char(111),char(99),char(101),char(115),char(115), +char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121), +char(48),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(49),char(0),char(109),char(95),char(103),char(105),char(109),char(112),char(97),char(99),char(116), +char(83),char(117),char(98),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80), +char(111),char(105),char(110),char(116),char(115),char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(117),char(110),char(115), +char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98),char(108),char(101),char(80),char(116),char(114), +char(0),char(109),char(95),char(110),char(117),char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115), +char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0),char(42),char(109),char(95),char(98),char(114), +char(111),char(97),char(100),char(112),char(104),char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0),char(42),char(109),char(95),char(99),char(111), +char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(42),char(109),char(95),char(114),char(111),char(111),char(116), +char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(109),char(95),char(119),char(111),char(114), +char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112), +char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114), +char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(76),char(105),char(110), +char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112), +char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105), +char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105),char(99), +char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(84), +char(105),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(114),char(111),char(108), +char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97), +char(99),char(116),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(99),char(116),char(83), +char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105), +char(111),char(110),char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99), +char(99),char(100),char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114),char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0), +char(109),char(95),char(99),char(99),char(100),char(77),char(111),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100), +char(0),char(109),char(95),char(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105), +char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97), +char(103),char(115),char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84),char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111), +char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105), +char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84), +char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105), +char(116),char(104),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(70),char(105),char(108),char(116),char(101),char(114), +char(71),char(114),char(111),char(117),char(112),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(70),char(105),char(108), +char(116),char(101),char(114),char(77),char(97),char(115),char(107),char(0),char(109),char(95),char(117),char(110),char(105),char(113),char(117),char(101),char(73),char(100),char(0),char(109), +char(95),char(116),char(97),char(117),char(0),char(109),char(95),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(116),char(105),char(109), +char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),char(120),char(69),char(114),char(114),char(111),char(114),char(82),char(101),char(100),char(117), +char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),char(114),char(0),char(109),char(95),char(101),char(114),char(112),char(0),char(109),char(95), +char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),char(98),char(97),char(108),char(67),char(102),char(109),char(0),char(109),char(95),char(115), +char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(80),char(101),char(110),char(101),char(116),char(114),char(97),char(116),char(105), +char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73), +char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),char(69),char(114),char(112),char(0),char(109),char(95),char(108),char(105),char(110),char(101), +char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),char(97),char(114),char(109),char(115),char(116),char(97),char(114),char(116),char(105),char(110), +char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(97),char(120),char(71),char(121),char(114),char(111),char(115),char(99),char(111), +char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(115),char(105),char(110),char(103),char(108),char(101),char(65),char(120),char(105), +char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101), +char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),char(109),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110), +char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(114),char(101),char(115), +char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105), +char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(109),char(105),char(110),char(105),char(109),char(117), +char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),char(99),char(104),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(115), +char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114), +char(73),char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(0),char(109),char(95),char(99),char(111),char(108), +char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105), +char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100), +char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95), +char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110), +char(103),char(117),char(108),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114), +char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99), +char(101),char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116), +char(105),char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101), +char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118), +char(101),char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109), +char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110), +char(103),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110), +char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108), +char(76),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111), +char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110), +char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), +char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103), +char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95), +char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104), +char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110), +char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111), +char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98), +char(65),char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116), +char(114),char(97),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(100),char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100), +char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115), +char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68),char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105), +char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101), +char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100),char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101), +char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116), +char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(98),char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108), +char(115),char(101),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(105),char(115),char(69),char(110),char(97),char(98), +char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112), +char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105), +char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95), +char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0), +char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102),char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65), +char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97), +char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111), +char(116),char(111),char(114),char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95), +char(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111), +char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105), +char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105),char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95), +char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116), +char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49), +char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115), +char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97), +char(110),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116), +char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0), +char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0), +char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0), +char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110),char(99),char(101), +char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116),char(70),char(111), +char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(54), +char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110),char(97),char(98), +char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),char(117),char(109), +char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105), +char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(68),char(97), +char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(66),char(111),char(117), +char(110),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0), +char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105), +char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97), +char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(84),char(97), +char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97), +char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110), +char(101),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(108),char(105),char(110), +char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109), +char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103), +char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),char(117),char(109), +char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101), +char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(101),char(114), +char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69), +char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110), +char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(76),char(105), +char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114), +char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93), +char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(66),char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(97), +char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117), +char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114), +char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(77),char(111), +char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(84),char(97),char(114),char(103), +char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114), +char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(97),char(110),char(103),char(117), +char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(97),char(110),char(103), +char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0), +char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105), +char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114), +char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(110), +char(97),char(98),char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), +char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110), +char(103),char(117),char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93), +char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102), +char(102),char(110),char(101),char(115),char(115),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110), +char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105), +char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(111),char(116),char(97),char(116),char(101),char(79),char(114),char(100), +char(101),char(114),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73), +char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83), +char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116), +char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(83),char(116),char(105),char(102), +char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(0),char(109),char(95), +char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(114),char(101),char(118),char(105),char(111),char(117),char(115),char(80), +char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109), +char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97),char(116),char(101),char(100),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95), +char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95),char(97),char(114),char(101),char(97),char(0),char(109),char(95),char(97),char(116),char(116),char(97), +char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(50),char(93),char(0), +char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110),char(103),char(116),char(104),char(0),char(109),char(95),char(98),char(98),char(101),char(110),char(100), +char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(51),char(93), +char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114),char(101),char(97),char(0),char(109),char(95),char(99),char(48),char(91),char(52),char(93),char(0), +char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(114), +char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(99),char(49),char(0),char(109),char(95),char(99),char(50),char(0), +char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(70),char(114),char(97),char(109),char(101),char(0),char(42),char(109), +char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100), +char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111),char(77),char(111),char(100),char(101),char(108),char(0),char(109),char(95),char(98),char(97),char(117), +char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95),char(100),char(114),char(97),char(103),char(0),char(109),char(95),char(108),char(105),char(102),char(116), +char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117),char(114),char(101),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),char(101), +char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105),char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109), +char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99),char(104),char(0),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(67),char(111), +char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(107),char(105),char(110),char(101), +char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109), +char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115), +char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95), +char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100), +char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108), +char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116), +char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0), +char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109), +char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110), +char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112), +char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101), +char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(109),char(97),char(120),char(86), +char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(99),char(97),char(108),char(101),char(0),char(109),char(95), +char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109), +char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0), +char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95), +char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95), +char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(97),char(113),char(113),char(0),char(109),char(95), +char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(42),char(109),char(95), +char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(80),char(111),char(115),char(105),char(116),char(105),char(111), +char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87),char(101),char(105),char(103),char(116),char(115),char(0),char(109),char(95),char(98),char(118),char(111), +char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(97),char(109), +char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(108),char(111),char(99),char(105),char(105),char(0),char(109),char(95),char(105),char(110),char(118), +char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95), +char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(108),char(118),char(0),char(109),char(95), +char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(114),char(101),char(102),char(115),char(0),char(42),char(109),char(95),char(110), +char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(115),char(115),char(101),char(115), +char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97),char(109),char(101),char(82),char(101),char(102),char(115),char(0),char(109),char(95),char(110),char(117), +char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(115),char(115),char(101),char(115),char(0),char(109), +char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(105),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(110),char(118), +char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101), +char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(100),char(97),char(109),char(112), +char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(116), +char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(120),char(83),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105), +char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(115),char(101),char(108),char(102),char(67),char(111), +char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(70),char(97),char(99),char(116),char(111),char(114), +char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105),char(110),char(115),char(65),char(110),char(99),char(104),char(111),char(114),char(0),char(109),char(95), +char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(110),char(100), +char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(66), +char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(99),char(102),char(109),char(0),char(109),char(95),char(115), +char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101),char(108),char(101),char(116),char(101),char(0),char(109),char(95),char(114),char(101),char(108),char(80), +char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50),char(93),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(116),char(121), +char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(106),char(111),char(105), +char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(101),char(0),char(42),char(42),char(109),char(95),char(109), +char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(115),char(0),char(42),char(109), +char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109),char(95),char(102),char(97),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(116), +char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(42),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(115), +char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110), +char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(109),char(95), +char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(97),char(99),char(101),char(115),char(0), +char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(109),char(95),char(110),char(117), +char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(108),char(117),char(115),char(116),char(101), +char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), +char(102),char(105),char(103),char(0),char(109),char(95),char(122),char(101),char(114),char(111),char(82),char(111),char(116),char(80),char(97),char(114),char(101),char(110),char(116),char(84), +char(111),char(84),char(104),char(105),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(67),char(111),char(109),char(84),char(111),char(84), +char(104),char(105),char(115),char(80),char(105),char(118),char(111),char(116),char(79),char(102),char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(116),char(104),char(105), +char(115),char(80),char(105),char(118),char(111),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),char(102),char(115),char(101), +char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(84),char(111),char(112),char(91),char(54),char(93),char(0), +char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(66),char(111),char(116),char(116),char(111),char(109),char(91),char(54),char(93), +char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(97),char(98),char(115), +char(70),char(114),char(97),char(109),char(101),char(84),char(111),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(84),char(111),char(112),char(0), +char(109),char(95),char(97),char(98),char(115),char(70),char(114),char(97),char(109),char(101),char(84),char(111),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116), +char(121),char(66),char(111),char(116),char(116),char(111),char(109),char(0),char(109),char(95),char(97),char(98),char(115),char(70),char(114),char(97),char(109),char(101),char(76),char(111), +char(99),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(84),char(111),char(112),char(0),char(109),char(95),char(97),char(98),char(115),char(70),char(114), +char(97),char(109),char(101),char(76),char(111),char(99),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(66),char(111),char(116),char(116),char(111),char(109), +char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116), +char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112), +char(111),char(115),char(86),char(97),char(114),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111), +char(115),char(91),char(55),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109), +char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(109),char(95),char(106),char(111), +char(105),char(110),char(116),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(70),char(114), +char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(76),char(111),char(119),char(101),char(114),char(76), +char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109), +char(105),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(77),char(97),char(120),char(70),char(111),char(114),char(99),char(101),char(0),char(109), +char(95),char(106),char(111),char(105),char(110),char(116),char(77),char(97),char(120),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(42),char(109), +char(95),char(108),char(105),char(110),char(107),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(78),char(97), +char(109),char(101),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(0),char(42), +char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(80),char(116),char(114),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87), +char(111),char(114),char(108),char(100),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87), +char(111),char(114),char(108),char(100),char(79),char(114),char(105),char(101),char(110),char(116),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(98),char(97), +char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(98), +char(97),char(115),char(101),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109), +char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(77), +char(97),char(115),char(115),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98), +char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(79),char(98),char(106), +char(68),char(97),char(116),char(97),char(0),char(42),char(109),char(95),char(109),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(0),char(109),char(95), +char(108),char(105),char(110),char(107),char(0),char(0),char(0),char(0),char(84),char(89),char(80),char(69),char(99),char(0),char(0),char(0),char(99),char(104),char(97),char(114), +char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0), +char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116), +char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114), +char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101), +char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51), +char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105), +char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114), +char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116), +char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97), +char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101), +char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101), +char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97), +char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104), +char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105), +char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99), +char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110), +char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115), +char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101), +char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105), +char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110), +char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103), +char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114), +char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104), +char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104), +char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111), +char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110), +char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83), +char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104), +char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110), +char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(101),char(114),char(115),char(105),char(115),char(116),char(101),char(110),char(116),char(77), +char(97),char(110),char(105),char(102),char(111),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108), +char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(101),char(114),char(115),char(105),char(115),char(116),char(101),char(110),char(116),char(77),char(97), +char(110),char(105),char(102),char(111),char(108),char(100),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), +char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112), +char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117),char(108),char(108),char(83),char(104), +char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108), +char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111), +char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114), +char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109), +char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),char(0),char(98), +char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97), +char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97), +char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), +char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50), +char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116), +char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111), +char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0), +char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97), +char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103), +char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70), +char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116), +char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115), +char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101), +char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98), +char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111), +char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114), +char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101), +char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114), +char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68), +char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67), +char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100), +char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70), +char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116), +char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116), +char(66),char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102), +char(116),char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111), +char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70), +char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114), +char(97),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111), +char(114),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103), +char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116), +char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(111),char(117), +char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76), +char(105),char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105), +char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108), +char(116),char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117), +char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(70), +char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121), +char(76),char(105),char(110),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0), +char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),char(12),char(0),char(36),char(0),char(8),char(0),char(16),char(0),char(32),char(0),char(16),char(0), +char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(84),char(0), +char(-124),char(0),char(12),char(0),char(52),char(0),char(52),char(0),char(20),char(0),char(64),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0), +char(32),char(0),char(28),char(0),char(60),char(0),char(56),char(0),char(76),char(0),char(76),char(0),char(24),char(0),char(60),char(0),char(60),char(0),char(60),char(0), +char(16),char(0),char(-16),char(5),char(-24),char(1),char(56),char(3),char(16),char(1),char(64),char(0),char(68),char(0),char(-104),char(0),char(88),char(0),char(-72),char(0), +char(104),char(0),char(-8),char(1),char(-72),char(3),char(8),char(0),char(52),char(0),char(52),char(0),char(0),char(0),char(68),char(0),char(84),char(0),char(-124),char(0), +char(116),char(0),char(92),char(1),char(-36),char(0),char(-116),char(1),char(124),char(1),char(-44),char(0),char(-4),char(0),char(-52),char(1),char(92),char(1),char(116),char(2), +char(-124),char(2),char(-76),char(4),char(-52),char(0),char(108),char(1),char(92),char(0),char(-116),char(0),char(16),char(0),char(100),char(0),char(20),char(0),char(36),char(0), +char(100),char(0),char(92),char(0),char(104),char(0),char(-64),char(0),char(92),char(1),char(104),char(0),char(-68),char(1),char(112),char(3),char(-56),char(1),char(-68),char(0), +char(100),char(0),char(28),char(1),char(-12),char(1),char(0),char(0),char(83),char(84),char(82),char(67),char(88),char(0),char(0),char(0),char(10),char(0),char(3),char(0), +char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0), +char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0), +char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0), +char(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0),char(13),char(0),char(9),char(0), +char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0),char(13),char(0),char(11),char(0), +char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0),char(4),char(0),char(12),char(0), +char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0),char(13),char(0),char(16),char(0), +char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0), +char(23),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0), +char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0), +char(4),char(0),char(22),char(0),char(25),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),char(13),char(0),char(25),char(0), +char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(22),char(0),char(30),char(0), +char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(26),char(0),char(12),char(0), +char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0), +char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(4),char(0),char(33),char(0), +char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(36),char(0), +char(0),char(0),char(37),char(0),char(28),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(40),char(0), +char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0), +char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0),char(13),char(0),char(45),char(0), +char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0),char(4),char(0),char(49),char(0), +char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0),char(2),char(0),char(50),char(0), +char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),char(35),char(0),char(2),char(0), +char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0),char(14),char(0),char(55),char(0), +char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0),char(4),char(0),char(60),char(0), +char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0),char(4),char(0),char(64),char(0), +char(0),char(0),char(37),char(0),char(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(25),char(0),char(66),char(0), +char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),char(40),char(0),char(2),char(0), +char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0),char(27),char(0),char(72),char(0), +char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0),char(41),char(0),char(75),char(0), +char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0), +char(0),char(0),char(37),char(0),char(44),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0),char(0),char(0),char(37),char(0), +char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(46),char(0),char(4),char(0), +char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0),char(39),char(0),char(14),char(0), +char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0),char(7),char(0),char(87),char(0), +char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0),char(4),char(0),char(92),char(0), +char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0),char(47),char(0),char(38),char(0), +char(14),char(0),char(96),char(0),char(14),char(0),char(97),char(0),char(14),char(0),char(98),char(0),char(14),char(0),char(99),char(0),char(14),char(0),char(100),char(0), +char(14),char(0),char(101),char(0),char(14),char(0),char(102),char(0),char(8),char(0),char(103),char(0),char(8),char(0),char(104),char(0),char(8),char(0),char(105),char(0), +char(8),char(0),char(106),char(0),char(8),char(0),char(107),char(0),char(8),char(0),char(108),char(0),char(4),char(0),char(109),char(0),char(4),char(0),char(110),char(0), +char(4),char(0),char(111),char(0),char(4),char(0),char(112),char(0),char(4),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0), +char(8),char(0),char(116),char(0),char(8),char(0),char(117),char(0),char(8),char(0),char(118),char(0),char(8),char(0),char(119),char(0),char(8),char(0),char(120),char(0), +char(8),char(0),char(121),char(0),char(8),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0), +char(4),char(0),char(126),char(0),char(4),char(0),char(127),char(0),char(4),char(0),char(-128),char(0),char(8),char(0),char(-127),char(0),char(8),char(0),char(-126),char(0), +char(4),char(0),char(44),char(0),char(48),char(0),char(-125),char(0),char(48),char(0),char(-124),char(0),char(49),char(0),char(38),char(0),char(13),char(0),char(96),char(0), +char(13),char(0),char(97),char(0),char(13),char(0),char(98),char(0),char(13),char(0),char(99),char(0),char(13),char(0),char(100),char(0),char(13),char(0),char(101),char(0), +char(13),char(0),char(102),char(0),char(7),char(0),char(103),char(0),char(7),char(0),char(104),char(0),char(7),char(0),char(105),char(0),char(7),char(0),char(106),char(0), +char(7),char(0),char(107),char(0),char(7),char(0),char(108),char(0),char(4),char(0),char(109),char(0),char(4),char(0),char(110),char(0),char(4),char(0),char(111),char(0), +char(4),char(0),char(112),char(0),char(4),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),char(7),char(0),char(116),char(0), +char(7),char(0),char(117),char(0),char(7),char(0),char(118),char(0),char(7),char(0),char(119),char(0),char(7),char(0),char(120),char(0),char(7),char(0),char(121),char(0), +char(7),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(4),char(0),char(126),char(0), +char(4),char(0),char(127),char(0),char(4),char(0),char(-128),char(0),char(7),char(0),char(-127),char(0),char(7),char(0),char(-126),char(0),char(4),char(0),char(44),char(0), +char(50),char(0),char(-125),char(0),char(50),char(0),char(-124),char(0),char(51),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0), +char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(-123),char(0),char(52),char(0),char(5),char(0),char(29),char(0),char(47),char(0), +char(13),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(4),char(0),char(-120),char(0),char(0),char(0),char(-119),char(0),char(48),char(0),char(29),char(0), +char(9),char(0),char(-118),char(0),char(9),char(0),char(-117),char(0),char(27),char(0),char(-116),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(-115),char(0), +char(20),char(0),char(-114),char(0),char(14),char(0),char(-113),char(0),char(14),char(0),char(-112),char(0),char(14),char(0),char(-111),char(0),char(8),char(0),char(-126),char(0), +char(8),char(0),char(-110),char(0),char(8),char(0),char(-109),char(0),char(8),char(0),char(-108),char(0),char(8),char(0),char(-107),char(0),char(8),char(0),char(-106),char(0), +char(8),char(0),char(-105),char(0),char(8),char(0),char(-104),char(0),char(8),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0),char(4),char(0),char(-101),char(0), +char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(4),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(4),char(0),char(-96),char(0), +char(4),char(0),char(-95),char(0),char(4),char(0),char(-94),char(0),char(4),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(50),char(0),char(29),char(0), +char(9),char(0),char(-118),char(0),char(9),char(0),char(-117),char(0),char(27),char(0),char(-116),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(-115),char(0), +char(19),char(0),char(-114),char(0),char(13),char(0),char(-113),char(0),char(13),char(0),char(-112),char(0),char(13),char(0),char(-111),char(0),char(7),char(0),char(-126),char(0), +char(7),char(0),char(-110),char(0),char(7),char(0),char(-109),char(0),char(7),char(0),char(-108),char(0),char(7),char(0),char(-107),char(0),char(7),char(0),char(-106),char(0), +char(7),char(0),char(-105),char(0),char(7),char(0),char(-104),char(0),char(7),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),char(4),char(0),char(-101),char(0), +char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(4),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(4),char(0),char(-96),char(0), +char(4),char(0),char(-95),char(0),char(4),char(0),char(-94),char(0),char(4),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(53),char(0),char(22),char(0), +char(8),char(0),char(-91),char(0),char(8),char(0),char(-90),char(0),char(8),char(0),char(-109),char(0),char(8),char(0),char(-89),char(0),char(8),char(0),char(-105),char(0), +char(8),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0),char(8),char(0),char(-85),char(0),char(8),char(0),char(-84),char(0), +char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),char(8),char(0),char(-80),char(0),char(8),char(0),char(-79),char(0), +char(8),char(0),char(-78),char(0),char(4),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0),char(4),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0), +char(4),char(0),char(-73),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(22),char(0),char(7),char(0),char(-91),char(0),char(7),char(0),char(-90),char(0), +char(7),char(0),char(-109),char(0),char(7),char(0),char(-89),char(0),char(7),char(0),char(-105),char(0),char(7),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0), +char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0), +char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(4),char(0),char(-77),char(0), +char(4),char(0),char(-76),char(0),char(4),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),char(0),char(0),char(37),char(0), +char(55),char(0),char(2),char(0),char(53),char(0),char(-72),char(0),char(14),char(0),char(-71),char(0),char(56),char(0),char(2),char(0),char(54),char(0),char(-72),char(0), +char(13),char(0),char(-71),char(0),char(57),char(0),char(21),char(0),char(50),char(0),char(-70),char(0),char(17),char(0),char(-69),char(0),char(13),char(0),char(-68),char(0), +char(13),char(0),char(-67),char(0),char(13),char(0),char(-66),char(0),char(13),char(0),char(-65),char(0),char(13),char(0),char(-71),char(0),char(13),char(0),char(-64),char(0), +char(13),char(0),char(-63),char(0),char(13),char(0),char(-62),char(0),char(13),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0),char(7),char(0),char(-59),char(0), +char(7),char(0),char(-58),char(0),char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0),char(7),char(0),char(-55),char(0),char(7),char(0),char(-54),char(0), +char(7),char(0),char(-53),char(0),char(7),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(58),char(0),char(22),char(0),char(48),char(0),char(-70),char(0), +char(18),char(0),char(-69),char(0),char(14),char(0),char(-68),char(0),char(14),char(0),char(-67),char(0),char(14),char(0),char(-66),char(0),char(14),char(0),char(-65),char(0), +char(14),char(0),char(-71),char(0),char(14),char(0),char(-64),char(0),char(14),char(0),char(-63),char(0),char(14),char(0),char(-62),char(0),char(14),char(0),char(-61),char(0), +char(8),char(0),char(-60),char(0),char(8),char(0),char(-59),char(0),char(8),char(0),char(-58),char(0),char(8),char(0),char(-57),char(0),char(8),char(0),char(-56),char(0), +char(8),char(0),char(-55),char(0),char(8),char(0),char(-54),char(0),char(8),char(0),char(-53),char(0),char(8),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0), +char(0),char(0),char(37),char(0),char(59),char(0),char(2),char(0),char(4),char(0),char(-50),char(0),char(4),char(0),char(-49),char(0),char(60),char(0),char(13),char(0), +char(57),char(0),char(-48),char(0),char(57),char(0),char(-47),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-128),char(0),char(4),char(0),char(-46),char(0), +char(4),char(0),char(-45),char(0),char(4),char(0),char(-44),char(0),char(7),char(0),char(-43),char(0),char(7),char(0),char(-42),char(0),char(4),char(0),char(-41),char(0), +char(4),char(0),char(-40),char(0),char(7),char(0),char(-39),char(0),char(4),char(0),char(-38),char(0),char(61),char(0),char(13),char(0),char(62),char(0),char(-48),char(0), +char(62),char(0),char(-47),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-128),char(0),char(4),char(0),char(-46),char(0),char(4),char(0),char(-45),char(0), +char(4),char(0),char(-44),char(0),char(7),char(0),char(-43),char(0),char(7),char(0),char(-42),char(0),char(4),char(0),char(-41),char(0),char(4),char(0),char(-40),char(0), +char(7),char(0),char(-39),char(0),char(4),char(0),char(-38),char(0),char(63),char(0),char(14),char(0),char(58),char(0),char(-48),char(0),char(58),char(0),char(-47),char(0), +char(0),char(0),char(35),char(0),char(4),char(0),char(-128),char(0),char(4),char(0),char(-46),char(0),char(4),char(0),char(-45),char(0),char(4),char(0),char(-44),char(0), +char(8),char(0),char(-43),char(0),char(8),char(0),char(-42),char(0),char(4),char(0),char(-41),char(0),char(4),char(0),char(-40),char(0),char(8),char(0),char(-39),char(0), +char(4),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(64),char(0),char(3),char(0),char(61),char(0),char(-36),char(0),char(13),char(0),char(-35),char(0), +char(13),char(0),char(-34),char(0),char(65),char(0),char(3),char(0),char(63),char(0),char(-36),char(0),char(14),char(0),char(-35),char(0),char(14),char(0),char(-34),char(0), +char(66),char(0),char(3),char(0),char(61),char(0),char(-36),char(0),char(14),char(0),char(-35),char(0),char(14),char(0),char(-34),char(0),char(67),char(0),char(13),char(0), +char(61),char(0),char(-36),char(0),char(20),char(0),char(-33),char(0),char(20),char(0),char(-32),char(0),char(4),char(0),char(-31),char(0),char(4),char(0),char(-30),char(0), +char(4),char(0),char(-29),char(0),char(7),char(0),char(-28),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0), +char(7),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0),char(68),char(0),char(13),char(0),char(61),char(0),char(-36),char(0), +char(19),char(0),char(-33),char(0),char(19),char(0),char(-32),char(0),char(4),char(0),char(-31),char(0),char(4),char(0),char(-30),char(0),char(4),char(0),char(-29),char(0), +char(7),char(0),char(-28),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),char(7),char(0),char(-24),char(0), +char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0),char(69),char(0),char(14),char(0),char(63),char(0),char(-36),char(0),char(20),char(0),char(-33),char(0), +char(20),char(0),char(-32),char(0),char(4),char(0),char(-31),char(0),char(4),char(0),char(-30),char(0),char(4),char(0),char(-29),char(0),char(8),char(0),char(-28),char(0), +char(8),char(0),char(-27),char(0),char(8),char(0),char(-26),char(0),char(8),char(0),char(-25),char(0),char(8),char(0),char(-24),char(0),char(8),char(0),char(-23),char(0), +char(8),char(0),char(-22),char(0),char(0),char(0),char(-21),char(0),char(70),char(0),char(10),char(0),char(63),char(0),char(-36),char(0),char(20),char(0),char(-33),char(0), +char(20),char(0),char(-32),char(0),char(8),char(0),char(-20),char(0),char(8),char(0),char(-19),char(0),char(8),char(0),char(-18),char(0),char(8),char(0),char(-24),char(0), +char(8),char(0),char(-23),char(0),char(8),char(0),char(-22),char(0),char(8),char(0),char(-90),char(0),char(71),char(0),char(11),char(0),char(61),char(0),char(-36),char(0), +char(19),char(0),char(-33),char(0),char(19),char(0),char(-32),char(0),char(7),char(0),char(-20),char(0),char(7),char(0),char(-19),char(0),char(7),char(0),char(-18),char(0), +char(7),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0),char(7),char(0),char(-90),char(0),char(0),char(0),char(21),char(0), +char(72),char(0),char(9),char(0),char(61),char(0),char(-36),char(0),char(19),char(0),char(-33),char(0),char(19),char(0),char(-32),char(0),char(13),char(0),char(-17),char(0), +char(13),char(0),char(-16),char(0),char(13),char(0),char(-15),char(0),char(13),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0), +char(73),char(0),char(9),char(0),char(63),char(0),char(-36),char(0),char(20),char(0),char(-33),char(0),char(20),char(0),char(-32),char(0),char(14),char(0),char(-17),char(0), +char(14),char(0),char(-16),char(0),char(14),char(0),char(-15),char(0),char(14),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0), +char(74),char(0),char(5),char(0),char(72),char(0),char(-11),char(0),char(4),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0),char(7),char(0),char(-8),char(0), +char(7),char(0),char(-7),char(0),char(75),char(0),char(5),char(0),char(73),char(0),char(-11),char(0),char(4),char(0),char(-10),char(0),char(8),char(0),char(-9),char(0), +char(8),char(0),char(-8),char(0),char(8),char(0),char(-7),char(0),char(76),char(0),char(41),char(0),char(61),char(0),char(-36),char(0),char(19),char(0),char(-33),char(0), +char(19),char(0),char(-32),char(0),char(13),char(0),char(-17),char(0),char(13),char(0),char(-16),char(0),char(13),char(0),char(-6),char(0),char(13),char(0),char(-5),char(0), +char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(13),char(0),char(-1),char(0),char(13),char(0),char(0),char(1), +char(13),char(0),char(1),char(1),char(13),char(0),char(2),char(1),char(13),char(0),char(3),char(1),char(13),char(0),char(4),char(1),char(0),char(0),char(5),char(1), +char(0),char(0),char(6),char(1),char(0),char(0),char(7),char(1),char(0),char(0),char(8),char(1),char(0),char(0),char(9),char(1),char(0),char(0),char(-21),char(0), +char(13),char(0),char(-15),char(0),char(13),char(0),char(-14),char(0),char(13),char(0),char(10),char(1),char(13),char(0),char(11),char(1),char(13),char(0),char(12),char(1), +char(13),char(0),char(13),char(1),char(13),char(0),char(14),char(1),char(13),char(0),char(15),char(1),char(13),char(0),char(16),char(1),char(13),char(0),char(17),char(1), +char(13),char(0),char(18),char(1),char(13),char(0),char(19),char(1),char(13),char(0),char(20),char(1),char(0),char(0),char(21),char(1),char(0),char(0),char(22),char(1), +char(0),char(0),char(23),char(1),char(0),char(0),char(24),char(1),char(0),char(0),char(25),char(1),char(4),char(0),char(26),char(1),char(77),char(0),char(41),char(0), +char(63),char(0),char(-36),char(0),char(20),char(0),char(-33),char(0),char(20),char(0),char(-32),char(0),char(14),char(0),char(-17),char(0),char(14),char(0),char(-16),char(0), +char(14),char(0),char(-6),char(0),char(14),char(0),char(-5),char(0),char(14),char(0),char(-4),char(0),char(14),char(0),char(-3),char(0),char(14),char(0),char(-2),char(0), +char(14),char(0),char(-1),char(0),char(14),char(0),char(0),char(1),char(14),char(0),char(1),char(1),char(14),char(0),char(2),char(1),char(14),char(0),char(3),char(1), +char(14),char(0),char(4),char(1),char(0),char(0),char(5),char(1),char(0),char(0),char(6),char(1),char(0),char(0),char(7),char(1),char(0),char(0),char(8),char(1), +char(0),char(0),char(9),char(1),char(0),char(0),char(-21),char(0),char(14),char(0),char(-15),char(0),char(14),char(0),char(-14),char(0),char(14),char(0),char(10),char(1), +char(14),char(0),char(11),char(1),char(14),char(0),char(12),char(1),char(14),char(0),char(13),char(1),char(14),char(0),char(14),char(1),char(14),char(0),char(15),char(1), +char(14),char(0),char(16),char(1),char(14),char(0),char(17),char(1),char(14),char(0),char(18),char(1),char(14),char(0),char(19),char(1),char(14),char(0),char(20),char(1), +char(0),char(0),char(21),char(1),char(0),char(0),char(22),char(1),char(0),char(0),char(23),char(1),char(0),char(0),char(24),char(1),char(0),char(0),char(25),char(1), +char(4),char(0),char(26),char(1),char(78),char(0),char(9),char(0),char(61),char(0),char(-36),char(0),char(19),char(0),char(-33),char(0),char(19),char(0),char(-32),char(0), +char(7),char(0),char(-17),char(0),char(7),char(0),char(-16),char(0),char(7),char(0),char(-15),char(0),char(7),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0), +char(4),char(0),char(-12),char(0),char(79),char(0),char(9),char(0),char(63),char(0),char(-36),char(0),char(20),char(0),char(-33),char(0),char(20),char(0),char(-32),char(0), +char(8),char(0),char(-17),char(0),char(8),char(0),char(-16),char(0),char(8),char(0),char(-15),char(0),char(8),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0), +char(4),char(0),char(-12),char(0),char(80),char(0),char(5),char(0),char(60),char(0),char(-36),char(0),char(13),char(0),char(27),char(1),char(13),char(0),char(28),char(1), +char(7),char(0),char(29),char(1),char(0),char(0),char(37),char(0),char(81),char(0),char(4),char(0),char(63),char(0),char(-36),char(0),char(14),char(0),char(27),char(1), +char(14),char(0),char(28),char(1),char(8),char(0),char(29),char(1),char(82),char(0),char(4),char(0),char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1), +char(7),char(0),char(32),char(1),char(4),char(0),char(79),char(0),char(83),char(0),char(10),char(0),char(82),char(0),char(33),char(1),char(13),char(0),char(34),char(1), +char(13),char(0),char(35),char(1),char(13),char(0),char(36),char(1),char(13),char(0),char(37),char(1),char(13),char(0),char(38),char(1),char(7),char(0),char(-60),char(0), +char(7),char(0),char(39),char(1),char(4),char(0),char(40),char(1),char(4),char(0),char(53),char(0),char(84),char(0),char(4),char(0),char(82),char(0),char(33),char(1), +char(4),char(0),char(41),char(1),char(7),char(0),char(42),char(1),char(4),char(0),char(43),char(1),char(85),char(0),char(4),char(0),char(13),char(0),char(38),char(1), +char(82),char(0),char(33),char(1),char(4),char(0),char(44),char(1),char(7),char(0),char(45),char(1),char(86),char(0),char(7),char(0),char(13),char(0),char(46),char(1), +char(82),char(0),char(33),char(1),char(4),char(0),char(47),char(1),char(7),char(0),char(48),char(1),char(7),char(0),char(49),char(1),char(7),char(0),char(50),char(1), +char(4),char(0),char(53),char(0),char(87),char(0),char(6),char(0),char(17),char(0),char(51),char(1),char(13),char(0),char(49),char(1),char(13),char(0),char(52),char(1), +char(62),char(0),char(53),char(1),char(4),char(0),char(54),char(1),char(7),char(0),char(50),char(1),char(88),char(0),char(26),char(0),char(4),char(0),char(55),char(1), +char(7),char(0),char(56),char(1),char(7),char(0),char(-90),char(0),char(7),char(0),char(57),char(1),char(7),char(0),char(58),char(1),char(7),char(0),char(59),char(1), +char(7),char(0),char(60),char(1),char(7),char(0),char(61),char(1),char(7),char(0),char(62),char(1),char(7),char(0),char(63),char(1),char(7),char(0),char(64),char(1), +char(7),char(0),char(65),char(1),char(7),char(0),char(66),char(1),char(7),char(0),char(67),char(1),char(7),char(0),char(68),char(1),char(7),char(0),char(69),char(1), +char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),char(7),char(0),char(72),char(1),char(7),char(0),char(73),char(1),char(7),char(0),char(74),char(1), +char(4),char(0),char(75),char(1),char(4),char(0),char(76),char(1),char(4),char(0),char(77),char(1),char(4),char(0),char(78),char(1),char(4),char(0),char(-100),char(0), +char(89),char(0),char(12),char(0),char(17),char(0),char(79),char(1),char(17),char(0),char(80),char(1),char(17),char(0),char(81),char(1),char(13),char(0),char(82),char(1), +char(13),char(0),char(83),char(1),char(7),char(0),char(84),char(1),char(4),char(0),char(85),char(1),char(4),char(0),char(86),char(1),char(4),char(0),char(87),char(1), +char(4),char(0),char(88),char(1),char(7),char(0),char(48),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(27),char(0),char(19),char(0),char(89),char(1), +char(17),char(0),char(90),char(1),char(17),char(0),char(91),char(1),char(13),char(0),char(82),char(1),char(13),char(0),char(92),char(1),char(13),char(0),char(93),char(1), +char(13),char(0),char(94),char(1),char(13),char(0),char(95),char(1),char(13),char(0),char(96),char(1),char(4),char(0),char(97),char(1),char(7),char(0),char(98),char(1), +char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),char(4),char(0),char(101),char(1),char(7),char(0),char(102),char(1),char(7),char(0),char(103),char(1), +char(4),char(0),char(104),char(1),char(4),char(0),char(105),char(1),char(7),char(0),char(106),char(1),char(7),char(0),char(107),char(1),char(7),char(0),char(108),char(1), +char(7),char(0),char(109),char(1),char(7),char(0),char(110),char(1),char(7),char(0),char(111),char(1),char(4),char(0),char(112),char(1),char(4),char(0),char(113),char(1), +char(4),char(0),char(114),char(1),char(91),char(0),char(12),char(0),char(9),char(0),char(115),char(1),char(9),char(0),char(116),char(1),char(13),char(0),char(117),char(1), +char(7),char(0),char(118),char(1),char(7),char(0),char(-86),char(0),char(7),char(0),char(119),char(1),char(4),char(0),char(120),char(1),char(13),char(0),char(121),char(1), +char(4),char(0),char(122),char(1),char(4),char(0),char(123),char(1),char(4),char(0),char(124),char(1),char(4),char(0),char(53),char(0),char(92),char(0),char(19),char(0), +char(50),char(0),char(-70),char(0),char(89),char(0),char(125),char(1),char(82),char(0),char(126),char(1),char(83),char(0),char(127),char(1),char(84),char(0),char(-128),char(1), +char(85),char(0),char(-127),char(1),char(86),char(0),char(-126),char(1),char(87),char(0),char(-125),char(1),char(90),char(0),char(-124),char(1),char(91),char(0),char(-123),char(1), +char(4),char(0),char(-122),char(1),char(4),char(0),char(100),char(1),char(4),char(0),char(-121),char(1),char(4),char(0),char(-120),char(1),char(4),char(0),char(-119),char(1), +char(4),char(0),char(-118),char(1),char(4),char(0),char(-117),char(1),char(4),char(0),char(-116),char(1),char(88),char(0),char(-115),char(1),char(93),char(0),char(28),char(0), +char(16),char(0),char(-114),char(1),char(14),char(0),char(-113),char(1),char(14),char(0),char(-112),char(1),char(14),char(0),char(-111),char(1),char(14),char(0),char(-110),char(1), +char(14),char(0),char(-109),char(1),char(14),char(0),char(-108),char(1),char(14),char(0),char(-107),char(1),char(14),char(0),char(-106),char(1),char(14),char(0),char(-105),char(1), +char(8),char(0),char(-104),char(1),char(4),char(0),char(-103),char(1),char(4),char(0),char(124),char(1),char(4),char(0),char(-102),char(1),char(4),char(0),char(-101),char(1), +char(8),char(0),char(-100),char(1),char(8),char(0),char(-99),char(1),char(8),char(0),char(-98),char(1),char(8),char(0),char(-97),char(1),char(8),char(0),char(-96),char(1), +char(8),char(0),char(-95),char(1),char(8),char(0),char(-94),char(1),char(8),char(0),char(-93),char(1),char(8),char(0),char(-92),char(1),char(0),char(0),char(-91),char(1), +char(0),char(0),char(-90),char(1),char(48),char(0),char(-89),char(1),char(0),char(0),char(-88),char(1),char(94),char(0),char(28),char(0),char(15),char(0),char(-114),char(1), +char(13),char(0),char(-113),char(1),char(13),char(0),char(-112),char(1),char(13),char(0),char(-111),char(1),char(13),char(0),char(-110),char(1),char(13),char(0),char(-109),char(1), +char(13),char(0),char(-108),char(1),char(13),char(0),char(-107),char(1),char(13),char(0),char(-106),char(1),char(13),char(0),char(-105),char(1),char(4),char(0),char(-102),char(1), +char(7),char(0),char(-104),char(1),char(4),char(0),char(-103),char(1),char(4),char(0),char(124),char(1),char(7),char(0),char(-100),char(1),char(7),char(0),char(-99),char(1), +char(7),char(0),char(-98),char(1),char(4),char(0),char(-101),char(1),char(7),char(0),char(-97),char(1),char(7),char(0),char(-96),char(1),char(7),char(0),char(-95),char(1), +char(7),char(0),char(-94),char(1),char(7),char(0),char(-93),char(1),char(7),char(0),char(-92),char(1),char(0),char(0),char(-91),char(1),char(0),char(0),char(-90),char(1), +char(50),char(0),char(-89),char(1),char(0),char(0),char(-88),char(1),char(95),char(0),char(11),char(0),char(14),char(0),char(-87),char(1),char(16),char(0),char(-86),char(1), +char(14),char(0),char(-85),char(1),char(14),char(0),char(-84),char(1),char(14),char(0),char(-83),char(1),char(8),char(0),char(-82),char(1),char(4),char(0),char(-121),char(1), +char(0),char(0),char(37),char(0),char(0),char(0),char(-81),char(1),char(93),char(0),char(-128),char(1),char(48),char(0),char(-80),char(1),char(96),char(0),char(10),char(0), +char(13),char(0),char(-87),char(1),char(15),char(0),char(-86),char(1),char(13),char(0),char(-85),char(1),char(13),char(0),char(-84),char(1),char(13),char(0),char(-83),char(1), +char(7),char(0),char(-82),char(1),char(4),char(0),char(-121),char(1),char(0),char(0),char(-81),char(1),char(94),char(0),char(-128),char(1),char(50),char(0),char(-80),char(1), +char(97),char(0),char(4),char(0),char(50),char(0),char(-79),char(1),char(96),char(0),char(-78),char(1),char(4),char(0),char(-77),char(1),char(0),char(0),char(37),char(0), +char(98),char(0),char(4),char(0),char(48),char(0),char(-79),char(1),char(95),char(0),char(-78),char(1),char(4),char(0),char(-77),char(1),char(0),char(0),char(37),char(0), +}; +int sBulletDNAlen= sizeof(sBulletDNAstr); diff --git a/extern/bullet2/src/LinearMath/btSerializer.h b/extern/bullet/src/LinearMath/btSerializer.h similarity index 96% rename from extern/bullet2/src/LinearMath/btSerializer.h rename to extern/bullet/src/LinearMath/btSerializer.h index 033895b1e5ec..39be3f810e49 100644 --- a/extern/bullet2/src/LinearMath/btSerializer.h +++ b/extern/bullet/src/LinearMath/btSerializer.h @@ -26,7 +26,7 @@ subject to the following restrictions: -///only the 32bit versions for now + extern char sBulletDNAstr[]; extern int sBulletDNAlen; extern char sBulletDNAstr64[]; @@ -62,7 +62,8 @@ enum btSerializationFlags { BT_SERIALIZE_NO_BVH = 1, BT_SERIALIZE_NO_TRIANGLEINFOMAP = 2, - BT_SERIALIZE_NO_DUPLICATE_ASSERT = 4 + BT_SERIALIZE_NO_DUPLICATE_ASSERT = 4, + BT_SERIALIZE_CONTACT_MANIFOLDS = 8, }; class btSerializer @@ -115,6 +116,7 @@ class btSerializer #define BT_MULTIBODY_CODE BT_MAKE_ID('M','B','D','Y') +#define BT_MB_LINKCOLLIDER_CODE BT_MAKE_ID('M','B','L','C') #define BT_SOFTBODY_CODE BT_MAKE_ID('S','B','D','Y') #define BT_COLLISIONOBJECT_CODE BT_MAKE_ID('C','O','B','J') #define BT_RIGIDBODY_CODE BT_MAKE_ID('R','B','D','Y') @@ -127,9 +129,9 @@ class btSerializer #define BT_SBMATERIAL_CODE BT_MAKE_ID('S','B','M','T') #define BT_SBNODE_CODE BT_MAKE_ID('S','B','N','D') #define BT_DYNAMICSWORLD_CODE BT_MAKE_ID('D','W','L','D') +#define BT_CONTACTMANIFOLD_CODE BT_MAKE_ID('C','O','N','T') #define BT_DNA_CODE BT_MAKE_ID('D','N','A','1') - struct btPointerUid { union @@ -391,7 +393,8 @@ class btDefaultSerializer : public btSerializer btDefaultSerializer(int totalSize=0, unsigned char* buffer=0) - :m_totalSize(totalSize), + :m_uniqueIdGenerator(0), + m_totalSize(totalSize), m_currentSize(0), m_dna(0), m_dnaLength(0), @@ -446,6 +449,26 @@ class btDefaultSerializer : public btSerializer btAlignedFree(m_dna); } + static int getMemoryDnaSizeInBytes() + { + const bool VOID_IS_8 = ((sizeof(void*) == 8)); + + if (VOID_IS_8) + { + return sBulletDNAlen64; + } + return sBulletDNAlen; + } + static const char* getMemoryDna() + { + const bool VOID_IS_8 = ((sizeof(void*) == 8)); + if (VOID_IS_8) + { + return (const char*)sBulletDNAstr64; + } + return (const char*)sBulletDNAstr; + } + void insertHeader() { writeHeader(m_buffer); @@ -484,7 +507,7 @@ class btDefaultSerializer : public btSerializer buffer[9] = '2'; buffer[10] = '8'; - buffer[11] = '4'; + buffer[11] = '8'; } @@ -541,6 +564,7 @@ class btDefaultSerializer : public btSerializer virtual void* getUniquePointer(void*oldPtr) { + btAssert(m_uniqueIdGenerator >= 0); if (!oldPtr) return 0; diff --git a/extern/bullet/src/LinearMath/btSerializer64.cpp b/extern/bullet/src/LinearMath/btSerializer64.cpp new file mode 100644 index 000000000000..0aa5cbf30e0b --- /dev/null +++ b/extern/bullet/src/LinearMath/btSerializer64.cpp @@ -0,0 +1,689 @@ +char sBulletDNAstr64[]= { +char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(-76),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), +char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95), +char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111), +char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(115),char(0),char(42),char(102),char(105),char(114),char(115),char(116),char(0),char(42),char(108),char(97),char(115), +char(116),char(0),char(109),char(95),char(102),char(108),char(111),char(97),char(116),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(108),char(91),char(51), +char(93),char(0),char(109),char(95),char(98),char(97),char(115),char(105),char(115),char(0),char(109),char(95),char(111),char(114),char(105),char(103),char(105),char(110),char(0),char(109), +char(95),char(114),char(111),char(111),char(116),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98), +char(116),char(114),char(101),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100), +char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(91),char(51),char(93),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122), +char(101),char(100),char(65),char(97),char(98),char(98),char(77),char(97),char(120),char(91),char(51),char(93),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77), +char(105),char(110),char(79),char(114),char(103),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77),char(97),char(120),char(79),char(114),char(103),char(0),char(109), +char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98),char(80),char(97), +char(114),char(116),char(0),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109), +char(95),char(112),char(97),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101), +char(120),char(79),char(114),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(98), +char(118),char(104),char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(0),char(109),char(95),char(98),char(118),char(104),char(65),char(97),char(98),char(98),char(77), +char(97),char(120),char(0),char(109),char(95),char(98),char(118),char(104),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110), +char(0),char(109),char(95),char(99),char(117),char(114),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(117),char(115), +char(101),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(110),char(117),char(109),char(67), +char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(76),char(101),char(97),char(102),char(78),char(111),char(100),char(101),char(115),char(0),char(109), +char(95),char(110),char(117),char(109),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117), +char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(99),char(111),char(110),char(116),char(105),char(103),char(117),char(111), +char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105), +char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116), +char(114),char(0),char(42),char(109),char(95),char(115),char(117),char(98),char(84),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(80),char(116),char(114),char(0), +char(109),char(95),char(116),char(114),char(97),char(118),char(101),char(114),char(115),char(97),char(108),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(110),char(117), +char(109),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(72),char(101),char(97),char(100),char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(110), +char(97),char(109),char(101),char(0),char(109),char(95),char(115),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(112),char(97), +char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110), +char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(83),char(99),char(97), +char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(101),char(78),char(111),char(114),char(109),char(97),char(108),char(0),char(109), +char(95),char(112),char(108),char(97),char(110),char(101),char(67),char(111),char(110),char(115),char(116),char(97),char(110),char(116),char(0),char(109),char(95),char(105),char(109),char(112), +char(108),char(105),char(99),char(105),char(116),char(83),char(104),char(97),char(112),char(101),char(68),char(105),char(109),char(101),char(110),char(115),char(105),char(111),char(110),char(115), +char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(109), +char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(111),char(115),char(0),char(109),char(95),char(114),char(97),char(100), +char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108), +char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(42),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(80),char(111), +char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(108),char(111),char(99), +char(97),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(83),char(105),char(122),char(101),char(0), +char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(0),char(109),char(95),char(112),char(97),char(100),char(91),char(50),char(93),char(0),char(109),char(95),char(118), +char(97),char(108),char(117),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(112),char(97),char(100),char(0),char(42),char(109),char(95),char(118),char(101), +char(114),char(116),char(105),char(99),char(101),char(115),char(51),char(102),char(0),char(42),char(109),char(95),char(118),char(101),char(114),char(116),char(105),char(99),char(101),char(115), +char(51),char(100),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(51),char(50),char(0),char(42),char(109),char(95),char(51), +char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(42),char(109),char(95),char(51),char(105),char(110),char(100),char(105),char(99),char(101), +char(115),char(56),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(109),char(95),char(110),char(117), +char(109),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(101),char(114),char(116), +char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(115),char(80),char(116),char(114), +char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(101),char(115),char(104), +char(80),char(97),char(114),char(116),char(115),char(0),char(109),char(95),char(109),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99), +char(101),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(70),char(108),char(111),char(97),char(116),char(66), +char(118),char(104),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(68),char(111),char(117),char(98),char(108), +char(101),char(66),char(118),char(104),char(0),char(42),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111), +char(77),char(97),char(112),char(0),char(109),char(95),char(112),char(97),char(100),char(51),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(114),char(105),char(109), +char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(116),char(114),char(97),char(110),char(115), +char(102),char(111),char(114),char(109),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(0),char(109), +char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104), +char(105),char(108),char(100),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104), +char(97),char(112),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(104),char(105),char(108),char(100),char(83),char(104),char(97), +char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(117),char(112),char(73),char(110),char(100), +char(101),char(120),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86), +char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103), +char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42), +char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101), +char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80), +char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95), +char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110), +char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114), +char(116),char(101),char(120),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(68), +char(105),char(115),char(116),char(97),char(110),char(99),char(101),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(122), +char(101),char(114),char(111),char(65),char(114),char(101),char(97),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110), +char(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83), +char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117), +char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(76),char(111), +char(99),char(97),char(108),char(80),char(111),char(105),char(110),char(116),char(65),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116), +char(67),char(97),char(99),char(104),char(101),char(76),char(111),char(99),char(97),char(108),char(80),char(111),char(105),char(110),char(116),char(66),char(91),char(52),char(93),char(0), +char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110), +char(87),char(111),char(114),char(108),char(100),char(79),char(110),char(65),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67), +char(97),char(99),char(104),char(101),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(79),char(110),char(66), +char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(78),char(111),char(114),char(109), +char(97),char(108),char(87),char(111),char(114),char(108),char(100),char(79),char(110),char(66),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110), +char(116),char(67),char(97),char(99),char(104),char(101),char(76),char(97),char(116),char(101),char(114),char(97),char(108),char(70),char(114),char(105),char(99),char(116),char(105),char(111), +char(110),char(68),char(105),char(114),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104), +char(101),char(76),char(97),char(116),char(101),char(114),char(97),char(108),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(68),char(105),char(114),char(50), +char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(68),char(105),char(115),char(116), +char(97),char(110),char(99),char(101),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101), +char(65),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(91),char(52),char(93),char(0),char(109),char(95), +char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(70),char(114), +char(105),char(99),char(116),char(105),char(111),char(110),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99), +char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105), +char(99),char(116),char(105),char(111),char(110),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104), +char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(83),char(112),char(105),char(110),char(110),char(105),char(110),char(103),char(70),char(114),char(105), +char(99),char(116),char(105),char(111),char(110),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104), +char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110), +char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(80),char(97),char(114),char(116), +char(73),char(100),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(80), +char(97),char(114),char(116),char(73),char(100),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99), +char(104),char(101),char(73),char(110),char(100),char(101),char(120),char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67), +char(97),char(99),char(104),char(101),char(73),char(110),char(100),char(101),char(120),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110), +char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(111),char(105),char(110),char(116),char(70),char(108), +char(97),char(103),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(65), +char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(76),char(97),char(116),char(101),char(114),char(97),char(108), +char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(65),char(112),char(112), +char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(76),char(97),char(116),char(101),char(114),char(97),char(108),char(50),char(91), +char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97), +char(99),char(116),char(77),char(111),char(116),char(105),char(111),char(110),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116), +char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(77),char(111),char(116),char(105),char(111),char(110),char(50),char(91), +char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(110),char(116),char(97), +char(99),char(116),char(67),char(70),char(77),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104), +char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(116),char(105),char(102), +char(102),char(110),char(101),char(115),char(115),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99), +char(104),char(101),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(69),char(82),char(80),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111), +char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(67),char(111),char(109),char(98),char(105),char(110),char(101),char(100),char(67),char(111),char(110),char(116), +char(97),char(99),char(116),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(112),char(111),char(105), +char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(67),char(70),char(77),char(91),char(52), +char(93),char(0),char(109),char(95),char(112),char(111),char(105),char(110),char(116),char(67),char(97),char(99),char(104),char(101),char(76),char(105),char(102),char(101),char(84),char(105), +char(109),char(101),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(97),char(99),char(104),char(101),char(100),char(80),char(111),char(105), +char(110),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(65),char(0),char(109), +char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(66),char(0),char(109),char(95),char(105),char(110),char(100),char(101), +char(120),char(49),char(97),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99), +char(111),char(110),char(116),char(97),char(99),char(116),char(66),char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104), +char(111),char(108),char(100),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(114),char(111),char(99),char(101),char(115),char(115), +char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121), +char(48),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(49),char(0),char(109),char(95),char(103),char(105),char(109),char(112),char(97),char(99),char(116), +char(83),char(117),char(98),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80), +char(111),char(105),char(110),char(116),char(115),char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(117),char(110),char(115), +char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98),char(108),char(101),char(80),char(116),char(114), +char(0),char(109),char(95),char(110),char(117),char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115), +char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0),char(42),char(109),char(95),char(98),char(114), +char(111),char(97),char(100),char(112),char(104),char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0),char(42),char(109),char(95),char(99),char(111), +char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(42),char(109),char(95),char(114),char(111),char(111),char(116), +char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(109),char(95),char(119),char(111),char(114), +char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112), +char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114), +char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(76),char(105),char(110), +char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112), +char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105), +char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105),char(99), +char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(84), +char(105),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(114),char(111),char(108), +char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97), +char(99),char(116),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(99),char(116),char(83), +char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105), +char(111),char(110),char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99), +char(99),char(100),char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114),char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0), +char(109),char(95),char(99),char(99),char(100),char(77),char(111),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100), +char(0),char(109),char(95),char(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105), +char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97), +char(103),char(115),char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84),char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111), +char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95),char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105), +char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84), +char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105), +char(116),char(104),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(70),char(105),char(108),char(116),char(101),char(114), +char(71),char(114),char(111),char(117),char(112),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(70),char(105),char(108), +char(116),char(101),char(114),char(77),char(97),char(115),char(107),char(0),char(109),char(95),char(117),char(110),char(105),char(113),char(117),char(101),char(73),char(100),char(0),char(109), +char(95),char(116),char(97),char(117),char(0),char(109),char(95),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(116),char(105),char(109), +char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),char(120),char(69),char(114),char(114),char(111),char(114),char(82),char(101),char(100),char(117), +char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),char(114),char(0),char(109),char(95),char(101),char(114),char(112),char(0),char(109),char(95), +char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),char(98),char(97),char(108),char(67),char(102),char(109),char(0),char(109),char(95),char(115), +char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(80),char(101),char(110),char(101),char(116),char(114),char(97),char(116),char(105), +char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73), +char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),char(69),char(114),char(112),char(0),char(109),char(95),char(108),char(105),char(110),char(101), +char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),char(97),char(114),char(109),char(115),char(116),char(97),char(114),char(116),char(105),char(110), +char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(97),char(120),char(71),char(121),char(114),char(111),char(115),char(99),char(111), +char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(115),char(105),char(110),char(103),char(108),char(101),char(65),char(120),char(105), +char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101), +char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),char(109),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110), +char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(114),char(101),char(115), +char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(82),char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105), +char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(109),char(105),char(110),char(105),char(109),char(117), +char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),char(99),char(104),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(115), +char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114), +char(73),char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(0),char(109),char(95),char(99),char(111),char(108), +char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105), +char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100), +char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95), +char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110), +char(103),char(117),char(108),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114), +char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99), +char(101),char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116), +char(105),char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101), +char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118), +char(101),char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109), +char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110), +char(103),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110), +char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108), +char(76),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111), +char(108),char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110), +char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), +char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103), +char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95), +char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104), +char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110), +char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111), +char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98), +char(65),char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116), +char(114),char(97),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(100),char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100), +char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97),char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115), +char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68),char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105), +char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101), +char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100),char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101), +char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116), +char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(98),char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108), +char(115),char(101),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(105),char(115),char(69),char(110),char(97),char(98), +char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112), +char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105), +char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95), +char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0), +char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102),char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65), +char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97), +char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111), +char(116),char(111),char(114),char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95), +char(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111), +char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105), +char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105),char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95), +char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116), +char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49), +char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115), +char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97), +char(110),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116), +char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0), +char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0), +char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0), +char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110),char(99),char(101), +char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116),char(70),char(111), +char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(54), +char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110),char(97),char(98), +char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),char(117),char(109), +char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105), +char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(68),char(97), +char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(66),char(111),char(117), +char(110),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0), +char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105), +char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97), +char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(84),char(97), +char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97), +char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110), +char(101),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(108),char(105),char(110), +char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109), +char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103), +char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105),char(117),char(109), +char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101), +char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(101),char(114), +char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69), +char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110), +char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(76),char(105), +char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114), +char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93), +char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(66),char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(97), +char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117), +char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114), +char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(77),char(111), +char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(84),char(97),char(114),char(103), +char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114), +char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(97),char(110),char(103),char(117), +char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(97),char(110),char(103), +char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0), +char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105), +char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114), +char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(110), +char(97),char(98),char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), +char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110), +char(103),char(117),char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93), +char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102), +char(102),char(110),char(101),char(115),char(115),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110), +char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105), +char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(111),char(116),char(97),char(116),char(101),char(79),char(114),char(100), +char(101),char(114),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73), +char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83), +char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116), +char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),char(101),char(83),char(116),char(105),char(102), +char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(0),char(109),char(95), +char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(114),char(101),char(118),char(105),char(111),char(117),char(115),char(80), +char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109), +char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97),char(116),char(101),char(100),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95), +char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95),char(97),char(114),char(101),char(97),char(0),char(109),char(95),char(97),char(116),char(116),char(97), +char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(50),char(93),char(0), +char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110),char(103),char(116),char(104),char(0),char(109),char(95),char(98),char(98),char(101),char(110),char(100), +char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(51),char(93), +char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114),char(101),char(97),char(0),char(109),char(95),char(99),char(48),char(91),char(52),char(93),char(0), +char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(114), +char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(99),char(49),char(0),char(109),char(95),char(99),char(50),char(0), +char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(70),char(114),char(97),char(109),char(101),char(0),char(42),char(109), +char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100), +char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111),char(77),char(111),char(100),char(101),char(108),char(0),char(109),char(95),char(98),char(97),char(117), +char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95),char(100),char(114),char(97),char(103),char(0),char(109),char(95),char(108),char(105),char(102),char(116), +char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117),char(114),char(101),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109),char(101), +char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105),char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109), +char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99),char(104),char(0),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(67),char(111), +char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(107),char(105),char(110),char(101), +char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109), +char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115), +char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95), +char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100), +char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108), +char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116), +char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0), +char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109), +char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110), +char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112), +char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101), +char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(109),char(97),char(120),char(86), +char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(99),char(97),char(108),char(101),char(0),char(109),char(95), +char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109), +char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0), +char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95), +char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95), +char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(97),char(113),char(113),char(0),char(109),char(95), +char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(42),char(109),char(95), +char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(80),char(111),char(115),char(105),char(116),char(105),char(111), +char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87),char(101),char(105),char(103),char(116),char(115),char(0),char(109),char(95),char(98),char(118),char(111), +char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(97),char(109), +char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(108),char(111),char(99),char(105),char(105),char(0),char(109),char(95),char(105),char(110),char(118), +char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95), +char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(108),char(118),char(0),char(109),char(95), +char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(114),char(101),char(102),char(115),char(0),char(42),char(109),char(95),char(110), +char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(115),char(115),char(101),char(115), +char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97),char(109),char(101),char(82),char(101),char(102),char(115),char(0),char(109),char(95),char(110),char(117), +char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(115),char(115),char(101),char(115),char(0),char(109), +char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(105),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(110),char(118), +char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101), +char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(100),char(97),char(109),char(112), +char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(116), +char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(120),char(83),char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105), +char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(115),char(101),char(108),char(102),char(67),char(111), +char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(70),char(97),char(99),char(116),char(111),char(114), +char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105),char(110),char(115),char(65),char(110),char(99),char(104),char(111),char(114),char(0),char(109),char(95), +char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(110),char(100), +char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(66), +char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(99),char(102),char(109),char(0),char(109),char(95),char(115), +char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101),char(108),char(101),char(116),char(101),char(0),char(109),char(95),char(114),char(101),char(108),char(80), +char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50),char(93),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(116),char(121), +char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(106),char(111),char(105), +char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(101),char(0),char(42),char(42),char(109),char(95),char(109), +char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(115),char(0),char(42),char(109), +char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109),char(95),char(102),char(97),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(116), +char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(42),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(115), +char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110), +char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(109),char(95), +char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(97),char(99),char(101),char(115),char(0), +char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(109),char(95),char(110),char(117), +char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(108),char(117),char(115),char(116),char(101), +char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), +char(102),char(105),char(103),char(0),char(109),char(95),char(122),char(101),char(114),char(111),char(82),char(111),char(116),char(80),char(97),char(114),char(101),char(110),char(116),char(84), +char(111),char(84),char(104),char(105),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(67),char(111),char(109),char(84),char(111),char(84), +char(104),char(105),char(115),char(80),char(105),char(118),char(111),char(116),char(79),char(102),char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(116),char(104),char(105), +char(115),char(80),char(105),char(118),char(111),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),char(102),char(115),char(101), +char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(84),char(111),char(112),char(91),char(54),char(93),char(0), +char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(66),char(111),char(116),char(116),char(111),char(109),char(91),char(54),char(93), +char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(97),char(98),char(115), +char(70),char(114),char(97),char(109),char(101),char(84),char(111),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(84),char(111),char(112),char(0), +char(109),char(95),char(97),char(98),char(115),char(70),char(114),char(97),char(109),char(101),char(84),char(111),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116), +char(121),char(66),char(111),char(116),char(116),char(111),char(109),char(0),char(109),char(95),char(97),char(98),char(115),char(70),char(114),char(97),char(109),char(101),char(76),char(111), +char(99),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(84),char(111),char(112),char(0),char(109),char(95),char(97),char(98),char(115),char(70),char(114), +char(97),char(109),char(101),char(76),char(111),char(99),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(66),char(111),char(116),char(116),char(111),char(109), +char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116), +char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112), +char(111),char(115),char(86),char(97),char(114),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111), +char(115),char(91),char(55),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109), +char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(109),char(95),char(106),char(111), +char(105),char(110),char(116),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(70),char(114), +char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(76),char(111),char(119),char(101),char(114),char(76), +char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109), +char(105),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(77),char(97),char(120),char(70),char(111),char(114),char(99),char(101),char(0),char(109), +char(95),char(106),char(111),char(105),char(110),char(116),char(77),char(97),char(120),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(42),char(109), +char(95),char(108),char(105),char(110),char(107),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(78),char(97), +char(109),char(101),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(0),char(42), +char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(80),char(116),char(114),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87), +char(111),char(114),char(108),char(100),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87), +char(111),char(114),char(108),char(100),char(79),char(114),char(105),char(101),char(110),char(116),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(98),char(97), +char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(98), +char(97),char(115),char(101),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109), +char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(77), +char(97),char(115),char(115),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98), +char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(79),char(98),char(106), +char(68),char(97),char(116),char(97),char(0),char(42),char(109),char(95),char(109),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(0),char(109),char(95), +char(108),char(105),char(110),char(107),char(0),char(0),char(0),char(0),char(84),char(89),char(80),char(69),char(99),char(0),char(0),char(0),char(99),char(104),char(97),char(114), +char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0), +char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116), +char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114), +char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101), +char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51), +char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105), +char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114), +char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116), +char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97), +char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101), +char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101), +char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97), +char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104), +char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105), +char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99), +char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110), +char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115), +char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101), +char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105), +char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110), +char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103), +char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114), +char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104), +char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104), +char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111), +char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110), +char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83), +char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104), +char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110), +char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(101),char(114),char(115),char(105),char(115),char(116),char(101),char(110),char(116),char(77), +char(97),char(110),char(105),char(102),char(111),char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108), +char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(101),char(114),char(115),char(105),char(115),char(116),char(101),char(110),char(116),char(77),char(97), +char(110),char(105),char(102),char(111),char(108),char(100),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111), +char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111),char(97),char(116),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112), +char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117),char(108),char(108),char(83),char(104), +char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108), +char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111), +char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114), +char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109), +char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), +char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98), +char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), +char(0),char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),char(0),char(98), +char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97), +char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97), +char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), +char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50), +char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116), +char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111), +char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0), +char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97), +char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103), +char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70), +char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115), +char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116), +char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115), +char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101), +char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68), +char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110), +char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98), +char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111), +char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114), +char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), +char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101), +char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114), +char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68), +char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68), +char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67), +char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100), +char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70), +char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116), +char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116), +char(66),char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102), +char(116),char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111), +char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70), +char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114), +char(97),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111), +char(114),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103), +char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116), +char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116), +char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(111),char(117), +char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76), +char(105),char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105), +char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108), +char(116),char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117), +char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(70), +char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121), +char(76),char(105),char(110),char(107),char(67),char(111),char(108),char(108),char(105),char(100),char(101),char(114),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), +char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78),char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0), +char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0),char(16),char(0),char(48),char(0),char(16),char(0),char(16),char(0),char(32),char(0),char(16),char(0), +char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0),char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(96),char(0), +char(-112),char(0),char(16),char(0),char(56),char(0),char(56),char(0),char(20),char(0),char(72),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0), +char(56),char(0),char(32),char(0),char(80),char(0),char(72),char(0),char(96),char(0),char(80),char(0),char(32),char(0),char(64),char(0),char(64),char(0),char(64),char(0), +char(16),char(0),char(-8),char(5),char(-8),char(1),char(64),char(3),char(32),char(1),char(72),char(0),char(80),char(0),char(-104),char(0),char(88),char(0),char(-72),char(0), +char(104),char(0),char(8),char(2),char(-56),char(3),char(8),char(0),char(64),char(0),char(64),char(0),char(0),char(0),char(80),char(0),char(96),char(0),char(-112),char(0), +char(-128),char(0),char(104),char(1),char(-24),char(0),char(-104),char(1),char(-120),char(1),char(-32),char(0),char(8),char(1),char(-40),char(1),char(104),char(1),char(-128),char(2), +char(-112),char(2),char(-64),char(4),char(-40),char(0),char(120),char(1),char(104),char(0),char(-104),char(0),char(16),char(0),char(104),char(0),char(24),char(0),char(40),char(0), +char(104),char(0),char(96),char(0),char(104),char(0),char(-56),char(0),char(104),char(1),char(112),char(0),char(-16),char(1),char(-128),char(3),char(-40),char(1),char(-56),char(0), +char(112),char(0),char(48),char(1),char(8),char(2),char(0),char(0),char(83),char(84),char(82),char(67),char(88),char(0),char(0),char(0),char(10),char(0),char(3),char(0), +char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0),char(10),char(0),char(3),char(0), +char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0),char(9),char(0),char(7),char(0), +char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(15),char(0),char(1),char(0), +char(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0),char(13),char(0),char(9),char(0), +char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0),char(13),char(0),char(11),char(0), +char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0),char(4),char(0),char(12),char(0), +char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0),char(13),char(0),char(16),char(0), +char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0), +char(23),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0), +char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0), +char(4),char(0),char(22),char(0),char(25),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0),char(13),char(0),char(25),char(0), +char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(22),char(0),char(30),char(0), +char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(26),char(0),char(12),char(0), +char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0), +char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(4),char(0),char(33),char(0), +char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(36),char(0), +char(0),char(0),char(37),char(0),char(28),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0),char(13),char(0),char(40),char(0), +char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0), +char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0),char(13),char(0),char(45),char(0), +char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0),char(4),char(0),char(49),char(0), +char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0),char(2),char(0),char(50),char(0), +char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0),char(35),char(0),char(2),char(0), +char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0),char(14),char(0),char(55),char(0), +char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0),char(4),char(0),char(60),char(0), +char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0),char(4),char(0),char(64),char(0), +char(0),char(0),char(37),char(0),char(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(25),char(0),char(66),char(0), +char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0),char(40),char(0),char(2),char(0), +char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0),char(27),char(0),char(72),char(0), +char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0),char(41),char(0),char(75),char(0), +char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0), +char(0),char(0),char(37),char(0),char(44),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0),char(0),char(0),char(37),char(0), +char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(46),char(0),char(4),char(0), +char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0),char(39),char(0),char(14),char(0), +char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0),char(7),char(0),char(87),char(0), +char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0),char(4),char(0),char(92),char(0), +char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0),char(47),char(0),char(38),char(0), +char(14),char(0),char(96),char(0),char(14),char(0),char(97),char(0),char(14),char(0),char(98),char(0),char(14),char(0),char(99),char(0),char(14),char(0),char(100),char(0), +char(14),char(0),char(101),char(0),char(14),char(0),char(102),char(0),char(8),char(0),char(103),char(0),char(8),char(0),char(104),char(0),char(8),char(0),char(105),char(0), +char(8),char(0),char(106),char(0),char(8),char(0),char(107),char(0),char(8),char(0),char(108),char(0),char(4),char(0),char(109),char(0),char(4),char(0),char(110),char(0), +char(4),char(0),char(111),char(0),char(4),char(0),char(112),char(0),char(4),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0), +char(8),char(0),char(116),char(0),char(8),char(0),char(117),char(0),char(8),char(0),char(118),char(0),char(8),char(0),char(119),char(0),char(8),char(0),char(120),char(0), +char(8),char(0),char(121),char(0),char(8),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0), +char(4),char(0),char(126),char(0),char(4),char(0),char(127),char(0),char(4),char(0),char(-128),char(0),char(8),char(0),char(-127),char(0),char(8),char(0),char(-126),char(0), +char(4),char(0),char(44),char(0),char(48),char(0),char(-125),char(0),char(48),char(0),char(-124),char(0),char(49),char(0),char(38),char(0),char(13),char(0),char(96),char(0), +char(13),char(0),char(97),char(0),char(13),char(0),char(98),char(0),char(13),char(0),char(99),char(0),char(13),char(0),char(100),char(0),char(13),char(0),char(101),char(0), +char(13),char(0),char(102),char(0),char(7),char(0),char(103),char(0),char(7),char(0),char(104),char(0),char(7),char(0),char(105),char(0),char(7),char(0),char(106),char(0), +char(7),char(0),char(107),char(0),char(7),char(0),char(108),char(0),char(4),char(0),char(109),char(0),char(4),char(0),char(110),char(0),char(4),char(0),char(111),char(0), +char(4),char(0),char(112),char(0),char(4),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0),char(7),char(0),char(116),char(0), +char(7),char(0),char(117),char(0),char(7),char(0),char(118),char(0),char(7),char(0),char(119),char(0),char(7),char(0),char(120),char(0),char(7),char(0),char(121),char(0), +char(7),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(4),char(0),char(124),char(0),char(4),char(0),char(125),char(0),char(4),char(0),char(126),char(0), +char(4),char(0),char(127),char(0),char(4),char(0),char(-128),char(0),char(7),char(0),char(-127),char(0),char(7),char(0),char(-126),char(0),char(4),char(0),char(44),char(0), +char(50),char(0),char(-125),char(0),char(50),char(0),char(-124),char(0),char(51),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0), +char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(-123),char(0),char(52),char(0),char(5),char(0),char(29),char(0),char(47),char(0), +char(13),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(4),char(0),char(-120),char(0),char(0),char(0),char(-119),char(0),char(48),char(0),char(29),char(0), +char(9),char(0),char(-118),char(0),char(9),char(0),char(-117),char(0),char(27),char(0),char(-116),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(-115),char(0), +char(20),char(0),char(-114),char(0),char(14),char(0),char(-113),char(0),char(14),char(0),char(-112),char(0),char(14),char(0),char(-111),char(0),char(8),char(0),char(-126),char(0), +char(8),char(0),char(-110),char(0),char(8),char(0),char(-109),char(0),char(8),char(0),char(-108),char(0),char(8),char(0),char(-107),char(0),char(8),char(0),char(-106),char(0), +char(8),char(0),char(-105),char(0),char(8),char(0),char(-104),char(0),char(8),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0),char(4),char(0),char(-101),char(0), +char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(4),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(4),char(0),char(-96),char(0), +char(4),char(0),char(-95),char(0),char(4),char(0),char(-94),char(0),char(4),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(50),char(0),char(29),char(0), +char(9),char(0),char(-118),char(0),char(9),char(0),char(-117),char(0),char(27),char(0),char(-116),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(-115),char(0), +char(19),char(0),char(-114),char(0),char(13),char(0),char(-113),char(0),char(13),char(0),char(-112),char(0),char(13),char(0),char(-111),char(0),char(7),char(0),char(-126),char(0), +char(7),char(0),char(-110),char(0),char(7),char(0),char(-109),char(0),char(7),char(0),char(-108),char(0),char(7),char(0),char(-107),char(0),char(7),char(0),char(-106),char(0), +char(7),char(0),char(-105),char(0),char(7),char(0),char(-104),char(0),char(7),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),char(4),char(0),char(-101),char(0), +char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(4),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(4),char(0),char(-96),char(0), +char(4),char(0),char(-95),char(0),char(4),char(0),char(-94),char(0),char(4),char(0),char(-93),char(0),char(4),char(0),char(-92),char(0),char(53),char(0),char(22),char(0), +char(8),char(0),char(-91),char(0),char(8),char(0),char(-90),char(0),char(8),char(0),char(-109),char(0),char(8),char(0),char(-89),char(0),char(8),char(0),char(-105),char(0), +char(8),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0),char(8),char(0),char(-85),char(0),char(8),char(0),char(-84),char(0), +char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0),char(8),char(0),char(-80),char(0),char(8),char(0),char(-79),char(0), +char(8),char(0),char(-78),char(0),char(4),char(0),char(-77),char(0),char(4),char(0),char(-76),char(0),char(4),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0), +char(4),char(0),char(-73),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(22),char(0),char(7),char(0),char(-91),char(0),char(7),char(0),char(-90),char(0), +char(7),char(0),char(-109),char(0),char(7),char(0),char(-89),char(0),char(7),char(0),char(-105),char(0),char(7),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0), +char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0), +char(7),char(0),char(-81),char(0),char(7),char(0),char(-80),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(4),char(0),char(-77),char(0), +char(4),char(0),char(-76),char(0),char(4),char(0),char(-75),char(0),char(4),char(0),char(-74),char(0),char(4),char(0),char(-73),char(0),char(0),char(0),char(37),char(0), +char(55),char(0),char(2),char(0),char(53),char(0),char(-72),char(0),char(14),char(0),char(-71),char(0),char(56),char(0),char(2),char(0),char(54),char(0),char(-72),char(0), +char(13),char(0),char(-71),char(0),char(57),char(0),char(21),char(0),char(50),char(0),char(-70),char(0),char(17),char(0),char(-69),char(0),char(13),char(0),char(-68),char(0), +char(13),char(0),char(-67),char(0),char(13),char(0),char(-66),char(0),char(13),char(0),char(-65),char(0),char(13),char(0),char(-71),char(0),char(13),char(0),char(-64),char(0), +char(13),char(0),char(-63),char(0),char(13),char(0),char(-62),char(0),char(13),char(0),char(-61),char(0),char(7),char(0),char(-60),char(0),char(7),char(0),char(-59),char(0), +char(7),char(0),char(-58),char(0),char(7),char(0),char(-57),char(0),char(7),char(0),char(-56),char(0),char(7),char(0),char(-55),char(0),char(7),char(0),char(-54),char(0), +char(7),char(0),char(-53),char(0),char(7),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0),char(58),char(0),char(22),char(0),char(48),char(0),char(-70),char(0), +char(18),char(0),char(-69),char(0),char(14),char(0),char(-68),char(0),char(14),char(0),char(-67),char(0),char(14),char(0),char(-66),char(0),char(14),char(0),char(-65),char(0), +char(14),char(0),char(-71),char(0),char(14),char(0),char(-64),char(0),char(14),char(0),char(-63),char(0),char(14),char(0),char(-62),char(0),char(14),char(0),char(-61),char(0), +char(8),char(0),char(-60),char(0),char(8),char(0),char(-59),char(0),char(8),char(0),char(-58),char(0),char(8),char(0),char(-57),char(0),char(8),char(0),char(-56),char(0), +char(8),char(0),char(-55),char(0),char(8),char(0),char(-54),char(0),char(8),char(0),char(-53),char(0),char(8),char(0),char(-52),char(0),char(4),char(0),char(-51),char(0), +char(0),char(0),char(37),char(0),char(59),char(0),char(2),char(0),char(4),char(0),char(-50),char(0),char(4),char(0),char(-49),char(0),char(60),char(0),char(13),char(0), +char(57),char(0),char(-48),char(0),char(57),char(0),char(-47),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-128),char(0),char(4),char(0),char(-46),char(0), +char(4),char(0),char(-45),char(0),char(4),char(0),char(-44),char(0),char(7),char(0),char(-43),char(0),char(7),char(0),char(-42),char(0),char(4),char(0),char(-41),char(0), +char(4),char(0),char(-40),char(0),char(7),char(0),char(-39),char(0),char(4),char(0),char(-38),char(0),char(61),char(0),char(13),char(0),char(62),char(0),char(-48),char(0), +char(62),char(0),char(-47),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-128),char(0),char(4),char(0),char(-46),char(0),char(4),char(0),char(-45),char(0), +char(4),char(0),char(-44),char(0),char(7),char(0),char(-43),char(0),char(7),char(0),char(-42),char(0),char(4),char(0),char(-41),char(0),char(4),char(0),char(-40),char(0), +char(7),char(0),char(-39),char(0),char(4),char(0),char(-38),char(0),char(63),char(0),char(14),char(0),char(58),char(0),char(-48),char(0),char(58),char(0),char(-47),char(0), +char(0),char(0),char(35),char(0),char(4),char(0),char(-128),char(0),char(4),char(0),char(-46),char(0),char(4),char(0),char(-45),char(0),char(4),char(0),char(-44),char(0), +char(8),char(0),char(-43),char(0),char(8),char(0),char(-42),char(0),char(4),char(0),char(-41),char(0),char(4),char(0),char(-40),char(0),char(8),char(0),char(-39),char(0), +char(4),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(64),char(0),char(3),char(0),char(61),char(0),char(-36),char(0),char(13),char(0),char(-35),char(0), +char(13),char(0),char(-34),char(0),char(65),char(0),char(3),char(0),char(63),char(0),char(-36),char(0),char(14),char(0),char(-35),char(0),char(14),char(0),char(-34),char(0), +char(66),char(0),char(3),char(0),char(61),char(0),char(-36),char(0),char(14),char(0),char(-35),char(0),char(14),char(0),char(-34),char(0),char(67),char(0),char(13),char(0), +char(61),char(0),char(-36),char(0),char(20),char(0),char(-33),char(0),char(20),char(0),char(-32),char(0),char(4),char(0),char(-31),char(0),char(4),char(0),char(-30),char(0), +char(4),char(0),char(-29),char(0),char(7),char(0),char(-28),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0), +char(7),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0),char(68),char(0),char(13),char(0),char(61),char(0),char(-36),char(0), +char(19),char(0),char(-33),char(0),char(19),char(0),char(-32),char(0),char(4),char(0),char(-31),char(0),char(4),char(0),char(-30),char(0),char(4),char(0),char(-29),char(0), +char(7),char(0),char(-28),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),char(7),char(0),char(-24),char(0), +char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0),char(69),char(0),char(14),char(0),char(63),char(0),char(-36),char(0),char(20),char(0),char(-33),char(0), +char(20),char(0),char(-32),char(0),char(4),char(0),char(-31),char(0),char(4),char(0),char(-30),char(0),char(4),char(0),char(-29),char(0),char(8),char(0),char(-28),char(0), +char(8),char(0),char(-27),char(0),char(8),char(0),char(-26),char(0),char(8),char(0),char(-25),char(0),char(8),char(0),char(-24),char(0),char(8),char(0),char(-23),char(0), +char(8),char(0),char(-22),char(0),char(0),char(0),char(-21),char(0),char(70),char(0),char(10),char(0),char(63),char(0),char(-36),char(0),char(20),char(0),char(-33),char(0), +char(20),char(0),char(-32),char(0),char(8),char(0),char(-20),char(0),char(8),char(0),char(-19),char(0),char(8),char(0),char(-18),char(0),char(8),char(0),char(-24),char(0), +char(8),char(0),char(-23),char(0),char(8),char(0),char(-22),char(0),char(8),char(0),char(-90),char(0),char(71),char(0),char(11),char(0),char(61),char(0),char(-36),char(0), +char(19),char(0),char(-33),char(0),char(19),char(0),char(-32),char(0),char(7),char(0),char(-20),char(0),char(7),char(0),char(-19),char(0),char(7),char(0),char(-18),char(0), +char(7),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0),char(7),char(0),char(-90),char(0),char(0),char(0),char(21),char(0), +char(72),char(0),char(9),char(0),char(61),char(0),char(-36),char(0),char(19),char(0),char(-33),char(0),char(19),char(0),char(-32),char(0),char(13),char(0),char(-17),char(0), +char(13),char(0),char(-16),char(0),char(13),char(0),char(-15),char(0),char(13),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0), +char(73),char(0),char(9),char(0),char(63),char(0),char(-36),char(0),char(20),char(0),char(-33),char(0),char(20),char(0),char(-32),char(0),char(14),char(0),char(-17),char(0), +char(14),char(0),char(-16),char(0),char(14),char(0),char(-15),char(0),char(14),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0), +char(74),char(0),char(5),char(0),char(72),char(0),char(-11),char(0),char(4),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0),char(7),char(0),char(-8),char(0), +char(7),char(0),char(-7),char(0),char(75),char(0),char(5),char(0),char(73),char(0),char(-11),char(0),char(4),char(0),char(-10),char(0),char(8),char(0),char(-9),char(0), +char(8),char(0),char(-8),char(0),char(8),char(0),char(-7),char(0),char(76),char(0),char(41),char(0),char(61),char(0),char(-36),char(0),char(19),char(0),char(-33),char(0), +char(19),char(0),char(-32),char(0),char(13),char(0),char(-17),char(0),char(13),char(0),char(-16),char(0),char(13),char(0),char(-6),char(0),char(13),char(0),char(-5),char(0), +char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(13),char(0),char(-1),char(0),char(13),char(0),char(0),char(1), +char(13),char(0),char(1),char(1),char(13),char(0),char(2),char(1),char(13),char(0),char(3),char(1),char(13),char(0),char(4),char(1),char(0),char(0),char(5),char(1), +char(0),char(0),char(6),char(1),char(0),char(0),char(7),char(1),char(0),char(0),char(8),char(1),char(0),char(0),char(9),char(1),char(0),char(0),char(-21),char(0), +char(13),char(0),char(-15),char(0),char(13),char(0),char(-14),char(0),char(13),char(0),char(10),char(1),char(13),char(0),char(11),char(1),char(13),char(0),char(12),char(1), +char(13),char(0),char(13),char(1),char(13),char(0),char(14),char(1),char(13),char(0),char(15),char(1),char(13),char(0),char(16),char(1),char(13),char(0),char(17),char(1), +char(13),char(0),char(18),char(1),char(13),char(0),char(19),char(1),char(13),char(0),char(20),char(1),char(0),char(0),char(21),char(1),char(0),char(0),char(22),char(1), +char(0),char(0),char(23),char(1),char(0),char(0),char(24),char(1),char(0),char(0),char(25),char(1),char(4),char(0),char(26),char(1),char(77),char(0),char(41),char(0), +char(63),char(0),char(-36),char(0),char(20),char(0),char(-33),char(0),char(20),char(0),char(-32),char(0),char(14),char(0),char(-17),char(0),char(14),char(0),char(-16),char(0), +char(14),char(0),char(-6),char(0),char(14),char(0),char(-5),char(0),char(14),char(0),char(-4),char(0),char(14),char(0),char(-3),char(0),char(14),char(0),char(-2),char(0), +char(14),char(0),char(-1),char(0),char(14),char(0),char(0),char(1),char(14),char(0),char(1),char(1),char(14),char(0),char(2),char(1),char(14),char(0),char(3),char(1), +char(14),char(0),char(4),char(1),char(0),char(0),char(5),char(1),char(0),char(0),char(6),char(1),char(0),char(0),char(7),char(1),char(0),char(0),char(8),char(1), +char(0),char(0),char(9),char(1),char(0),char(0),char(-21),char(0),char(14),char(0),char(-15),char(0),char(14),char(0),char(-14),char(0),char(14),char(0),char(10),char(1), +char(14),char(0),char(11),char(1),char(14),char(0),char(12),char(1),char(14),char(0),char(13),char(1),char(14),char(0),char(14),char(1),char(14),char(0),char(15),char(1), +char(14),char(0),char(16),char(1),char(14),char(0),char(17),char(1),char(14),char(0),char(18),char(1),char(14),char(0),char(19),char(1),char(14),char(0),char(20),char(1), +char(0),char(0),char(21),char(1),char(0),char(0),char(22),char(1),char(0),char(0),char(23),char(1),char(0),char(0),char(24),char(1),char(0),char(0),char(25),char(1), +char(4),char(0),char(26),char(1),char(78),char(0),char(9),char(0),char(61),char(0),char(-36),char(0),char(19),char(0),char(-33),char(0),char(19),char(0),char(-32),char(0), +char(7),char(0),char(-17),char(0),char(7),char(0),char(-16),char(0),char(7),char(0),char(-15),char(0),char(7),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0), +char(4),char(0),char(-12),char(0),char(79),char(0),char(9),char(0),char(63),char(0),char(-36),char(0),char(20),char(0),char(-33),char(0),char(20),char(0),char(-32),char(0), +char(8),char(0),char(-17),char(0),char(8),char(0),char(-16),char(0),char(8),char(0),char(-15),char(0),char(8),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0), +char(4),char(0),char(-12),char(0),char(80),char(0),char(5),char(0),char(60),char(0),char(-36),char(0),char(13),char(0),char(27),char(1),char(13),char(0),char(28),char(1), +char(7),char(0),char(29),char(1),char(0),char(0),char(37),char(0),char(81),char(0),char(4),char(0),char(63),char(0),char(-36),char(0),char(14),char(0),char(27),char(1), +char(14),char(0),char(28),char(1),char(8),char(0),char(29),char(1),char(82),char(0),char(4),char(0),char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1), +char(7),char(0),char(32),char(1),char(4),char(0),char(79),char(0),char(83),char(0),char(10),char(0),char(82),char(0),char(33),char(1),char(13),char(0),char(34),char(1), +char(13),char(0),char(35),char(1),char(13),char(0),char(36),char(1),char(13),char(0),char(37),char(1),char(13),char(0),char(38),char(1),char(7),char(0),char(-60),char(0), +char(7),char(0),char(39),char(1),char(4),char(0),char(40),char(1),char(4),char(0),char(53),char(0),char(84),char(0),char(4),char(0),char(82),char(0),char(33),char(1), +char(4),char(0),char(41),char(1),char(7),char(0),char(42),char(1),char(4),char(0),char(43),char(1),char(85),char(0),char(4),char(0),char(13),char(0),char(38),char(1), +char(82),char(0),char(33),char(1),char(4),char(0),char(44),char(1),char(7),char(0),char(45),char(1),char(86),char(0),char(7),char(0),char(13),char(0),char(46),char(1), +char(82),char(0),char(33),char(1),char(4),char(0),char(47),char(1),char(7),char(0),char(48),char(1),char(7),char(0),char(49),char(1),char(7),char(0),char(50),char(1), +char(4),char(0),char(53),char(0),char(87),char(0),char(6),char(0),char(17),char(0),char(51),char(1),char(13),char(0),char(49),char(1),char(13),char(0),char(52),char(1), +char(62),char(0),char(53),char(1),char(4),char(0),char(54),char(1),char(7),char(0),char(50),char(1),char(88),char(0),char(26),char(0),char(4),char(0),char(55),char(1), +char(7),char(0),char(56),char(1),char(7),char(0),char(-90),char(0),char(7),char(0),char(57),char(1),char(7),char(0),char(58),char(1),char(7),char(0),char(59),char(1), +char(7),char(0),char(60),char(1),char(7),char(0),char(61),char(1),char(7),char(0),char(62),char(1),char(7),char(0),char(63),char(1),char(7),char(0),char(64),char(1), +char(7),char(0),char(65),char(1),char(7),char(0),char(66),char(1),char(7),char(0),char(67),char(1),char(7),char(0),char(68),char(1),char(7),char(0),char(69),char(1), +char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),char(7),char(0),char(72),char(1),char(7),char(0),char(73),char(1),char(7),char(0),char(74),char(1), +char(4),char(0),char(75),char(1),char(4),char(0),char(76),char(1),char(4),char(0),char(77),char(1),char(4),char(0),char(78),char(1),char(4),char(0),char(-100),char(0), +char(89),char(0),char(12),char(0),char(17),char(0),char(79),char(1),char(17),char(0),char(80),char(1),char(17),char(0),char(81),char(1),char(13),char(0),char(82),char(1), +char(13),char(0),char(83),char(1),char(7),char(0),char(84),char(1),char(4),char(0),char(85),char(1),char(4),char(0),char(86),char(1),char(4),char(0),char(87),char(1), +char(4),char(0),char(88),char(1),char(7),char(0),char(48),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(27),char(0),char(19),char(0),char(89),char(1), +char(17),char(0),char(90),char(1),char(17),char(0),char(91),char(1),char(13),char(0),char(82),char(1),char(13),char(0),char(92),char(1),char(13),char(0),char(93),char(1), +char(13),char(0),char(94),char(1),char(13),char(0),char(95),char(1),char(13),char(0),char(96),char(1),char(4),char(0),char(97),char(1),char(7),char(0),char(98),char(1), +char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),char(4),char(0),char(101),char(1),char(7),char(0),char(102),char(1),char(7),char(0),char(103),char(1), +char(4),char(0),char(104),char(1),char(4),char(0),char(105),char(1),char(7),char(0),char(106),char(1),char(7),char(0),char(107),char(1),char(7),char(0),char(108),char(1), +char(7),char(0),char(109),char(1),char(7),char(0),char(110),char(1),char(7),char(0),char(111),char(1),char(4),char(0),char(112),char(1),char(4),char(0),char(113),char(1), +char(4),char(0),char(114),char(1),char(91),char(0),char(12),char(0),char(9),char(0),char(115),char(1),char(9),char(0),char(116),char(1),char(13),char(0),char(117),char(1), +char(7),char(0),char(118),char(1),char(7),char(0),char(-86),char(0),char(7),char(0),char(119),char(1),char(4),char(0),char(120),char(1),char(13),char(0),char(121),char(1), +char(4),char(0),char(122),char(1),char(4),char(0),char(123),char(1),char(4),char(0),char(124),char(1),char(4),char(0),char(53),char(0),char(92),char(0),char(19),char(0), +char(50),char(0),char(-70),char(0),char(89),char(0),char(125),char(1),char(82),char(0),char(126),char(1),char(83),char(0),char(127),char(1),char(84),char(0),char(-128),char(1), +char(85),char(0),char(-127),char(1),char(86),char(0),char(-126),char(1),char(87),char(0),char(-125),char(1),char(90),char(0),char(-124),char(1),char(91),char(0),char(-123),char(1), +char(4),char(0),char(-122),char(1),char(4),char(0),char(100),char(1),char(4),char(0),char(-121),char(1),char(4),char(0),char(-120),char(1),char(4),char(0),char(-119),char(1), +char(4),char(0),char(-118),char(1),char(4),char(0),char(-117),char(1),char(4),char(0),char(-116),char(1),char(88),char(0),char(-115),char(1),char(93),char(0),char(28),char(0), +char(16),char(0),char(-114),char(1),char(14),char(0),char(-113),char(1),char(14),char(0),char(-112),char(1),char(14),char(0),char(-111),char(1),char(14),char(0),char(-110),char(1), +char(14),char(0),char(-109),char(1),char(14),char(0),char(-108),char(1),char(14),char(0),char(-107),char(1),char(14),char(0),char(-106),char(1),char(14),char(0),char(-105),char(1), +char(8),char(0),char(-104),char(1),char(4),char(0),char(-103),char(1),char(4),char(0),char(124),char(1),char(4),char(0),char(-102),char(1),char(4),char(0),char(-101),char(1), +char(8),char(0),char(-100),char(1),char(8),char(0),char(-99),char(1),char(8),char(0),char(-98),char(1),char(8),char(0),char(-97),char(1),char(8),char(0),char(-96),char(1), +char(8),char(0),char(-95),char(1),char(8),char(0),char(-94),char(1),char(8),char(0),char(-93),char(1),char(8),char(0),char(-92),char(1),char(0),char(0),char(-91),char(1), +char(0),char(0),char(-90),char(1),char(48),char(0),char(-89),char(1),char(0),char(0),char(-88),char(1),char(94),char(0),char(28),char(0),char(15),char(0),char(-114),char(1), +char(13),char(0),char(-113),char(1),char(13),char(0),char(-112),char(1),char(13),char(0),char(-111),char(1),char(13),char(0),char(-110),char(1),char(13),char(0),char(-109),char(1), +char(13),char(0),char(-108),char(1),char(13),char(0),char(-107),char(1),char(13),char(0),char(-106),char(1),char(13),char(0),char(-105),char(1),char(4),char(0),char(-102),char(1), +char(7),char(0),char(-104),char(1),char(4),char(0),char(-103),char(1),char(4),char(0),char(124),char(1),char(7),char(0),char(-100),char(1),char(7),char(0),char(-99),char(1), +char(7),char(0),char(-98),char(1),char(4),char(0),char(-101),char(1),char(7),char(0),char(-97),char(1),char(7),char(0),char(-96),char(1),char(7),char(0),char(-95),char(1), +char(7),char(0),char(-94),char(1),char(7),char(0),char(-93),char(1),char(7),char(0),char(-92),char(1),char(0),char(0),char(-91),char(1),char(0),char(0),char(-90),char(1), +char(50),char(0),char(-89),char(1),char(0),char(0),char(-88),char(1),char(95),char(0),char(11),char(0),char(14),char(0),char(-87),char(1),char(16),char(0),char(-86),char(1), +char(14),char(0),char(-85),char(1),char(14),char(0),char(-84),char(1),char(14),char(0),char(-83),char(1),char(8),char(0),char(-82),char(1),char(4),char(0),char(-121),char(1), +char(0),char(0),char(37),char(0),char(0),char(0),char(-81),char(1),char(93),char(0),char(-128),char(1),char(48),char(0),char(-80),char(1),char(96),char(0),char(10),char(0), +char(13),char(0),char(-87),char(1),char(15),char(0),char(-86),char(1),char(13),char(0),char(-85),char(1),char(13),char(0),char(-84),char(1),char(13),char(0),char(-83),char(1), +char(7),char(0),char(-82),char(1),char(4),char(0),char(-121),char(1),char(0),char(0),char(-81),char(1),char(94),char(0),char(-128),char(1),char(50),char(0),char(-80),char(1), +char(97),char(0),char(4),char(0),char(50),char(0),char(-79),char(1),char(96),char(0),char(-78),char(1),char(4),char(0),char(-77),char(1),char(0),char(0),char(37),char(0), +char(98),char(0),char(4),char(0),char(48),char(0),char(-79),char(1),char(95),char(0),char(-78),char(1),char(4),char(0),char(-77),char(1),char(0),char(0),char(37),char(0), +}; +int sBulletDNAlen64= sizeof(sBulletDNAstr64); diff --git a/extern/bullet2/src/LinearMath/btSpatialAlgebra.h b/extern/bullet/src/LinearMath/btSpatialAlgebra.h similarity index 100% rename from extern/bullet2/src/LinearMath/btSpatialAlgebra.h rename to extern/bullet/src/LinearMath/btSpatialAlgebra.h diff --git a/extern/bullet2/src/LinearMath/btStackAlloc.h b/extern/bullet/src/LinearMath/btStackAlloc.h similarity index 100% rename from extern/bullet2/src/LinearMath/btStackAlloc.h rename to extern/bullet/src/LinearMath/btStackAlloc.h diff --git a/extern/bullet/src/LinearMath/btThreads.cpp b/extern/bullet/src/LinearMath/btThreads.cpp new file mode 100644 index 000000000000..c037626ffb0e --- /dev/null +++ b/extern/bullet/src/LinearMath/btThreads.cpp @@ -0,0 +1,819 @@ +/* +Copyright (c) 2003-2014 Erwin Coumans http://bullet.googlecode.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 "btThreads.h" +#include "btQuickprof.h" +#include // for min and max + + +#if BT_USE_OPENMP && BT_THREADSAFE + +#include + +#endif // #if BT_USE_OPENMP && BT_THREADSAFE + + +#if BT_USE_PPL && BT_THREADSAFE + +// use Microsoft Parallel Patterns Library (installed with Visual Studio 2010 and later) +#include // if you get a compile error here, check whether your version of Visual Studio includes PPL +// Visual Studio 2010 and later should come with it +#include // for GetProcessorCount() + +#endif // #if BT_USE_PPL && BT_THREADSAFE + + +#if BT_USE_TBB && BT_THREADSAFE + +// use Intel Threading Building Blocks for thread management +#define __TBB_NO_IMPLICIT_LINKAGE 1 +#include +#include +#include +#include + +#endif // #if BT_USE_TBB && BT_THREADSAFE + + +#if BT_THREADSAFE +// +// Lightweight spin-mutex based on atomics +// Using ordinary system-provided mutexes like Windows critical sections was noticeably slower +// presumably because when it fails to lock at first it would sleep the thread and trigger costly +// context switching. +// + +#if __cplusplus >= 201103L + +// for anything claiming full C++11 compliance, use C++11 atomics +// on GCC or Clang you need to compile with -std=c++11 +#define USE_CPP11_ATOMICS 1 + +#elif defined( _MSC_VER ) + +// on MSVC, use intrinsics instead +#define USE_MSVC_INTRINSICS 1 + +#elif defined( __GNUC__ ) && (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 7)) + +// available since GCC 4.7 and some versions of clang +// todo: check for clang +#define USE_GCC_BUILTIN_ATOMICS 1 + +#elif defined( __GNUC__ ) && (__GNUC__ == 4 && __GNUC_MINOR__ >= 1) + +// available since GCC 4.1 +#define USE_GCC_BUILTIN_ATOMICS_OLD 1 + +#endif + + +#if USE_CPP11_ATOMICS + +#include +#include + +#define THREAD_LOCAL_STATIC thread_local static + +bool btSpinMutex::tryLock() +{ + std::atomic* aDest = reinterpret_cast*>(&mLock); + int expected = 0; + return std::atomic_compare_exchange_weak_explicit( aDest, &expected, int(1), std::memory_order_acq_rel, std::memory_order_acquire ); +} + +void btSpinMutex::lock() +{ + // note: this lock does not sleep the thread. + while (! tryLock()) + { + // spin + } +} + +void btSpinMutex::unlock() +{ + std::atomic* aDest = reinterpret_cast*>(&mLock); + std::atomic_store_explicit( aDest, int(0), std::memory_order_release ); +} + + +#elif USE_MSVC_INTRINSICS + +#define WIN32_LEAN_AND_MEAN + +#include +#include + +#define THREAD_LOCAL_STATIC __declspec( thread ) static + + +bool btSpinMutex::tryLock() +{ + volatile long* aDest = reinterpret_cast(&mLock); + return ( 0 == _InterlockedCompareExchange( aDest, 1, 0) ); +} + +void btSpinMutex::lock() +{ + // note: this lock does not sleep the thread + while (! tryLock()) + { + // spin + } +} + +void btSpinMutex::unlock() +{ + volatile long* aDest = reinterpret_cast( &mLock ); + _InterlockedExchange( aDest, 0 ); +} + +#elif USE_GCC_BUILTIN_ATOMICS + +#define THREAD_LOCAL_STATIC static __thread + + +bool btSpinMutex::tryLock() +{ + int expected = 0; + bool weak = false; + const int memOrderSuccess = __ATOMIC_ACQ_REL; + const int memOrderFail = __ATOMIC_ACQUIRE; + return __atomic_compare_exchange_n(&mLock, &expected, int(1), weak, memOrderSuccess, memOrderFail); +} + +void btSpinMutex::lock() +{ + // note: this lock does not sleep the thread + while (! tryLock()) + { + // spin + } +} + +void btSpinMutex::unlock() +{ + __atomic_store_n(&mLock, int(0), __ATOMIC_RELEASE); +} + +#elif USE_GCC_BUILTIN_ATOMICS_OLD + + +#define THREAD_LOCAL_STATIC static __thread + +bool btSpinMutex::tryLock() +{ + return __sync_bool_compare_and_swap(&mLock, int(0), int(1)); +} + +void btSpinMutex::lock() +{ + // note: this lock does not sleep the thread + while (! tryLock()) + { + // spin + } +} + +void btSpinMutex::unlock() +{ + // write 0 + __sync_fetch_and_and(&mLock, int(0)); +} + +#else //#elif USE_MSVC_INTRINSICS + +#error "no threading primitives defined -- unknown platform" + +#endif //#else //#elif USE_MSVC_INTRINSICS + +#else //#if BT_THREADSAFE + +// These should not be called ever +void btSpinMutex::lock() +{ + btAssert( !"unimplemented btSpinMutex::lock() called" ); +} + +void btSpinMutex::unlock() +{ + btAssert( !"unimplemented btSpinMutex::unlock() called" ); +} + +bool btSpinMutex::tryLock() +{ + btAssert( !"unimplemented btSpinMutex::tryLock() called" ); + return true; +} + +#define THREAD_LOCAL_STATIC static + +#endif // #else //#if BT_THREADSAFE + + +struct ThreadsafeCounter +{ + unsigned int mCounter; + btSpinMutex mMutex; + + ThreadsafeCounter() + { + mCounter = 0; + --mCounter; // first count should come back 0 + } + + unsigned int getNext() + { + // no need to optimize this with atomics, it is only called ONCE per thread! + mMutex.lock(); + mCounter++; + if ( mCounter >= BT_MAX_THREAD_COUNT ) + { + btAssert( !"thread counter exceeded" ); + // wrap back to the first worker index + mCounter = 1; + } + unsigned int val = mCounter; + mMutex.unlock(); + return val; + } +}; + + +static btITaskScheduler* gBtTaskScheduler; +static int gThreadsRunningCounter = 0; // useful for detecting if we are trying to do nested parallel-for calls +static btSpinMutex gThreadsRunningCounterMutex; +static ThreadsafeCounter gThreadCounter; + + +// +// BT_DETECT_BAD_THREAD_INDEX tries to detect when there are multiple threads assigned the same thread index. +// +// BT_DETECT_BAD_THREAD_INDEX is a developer option to test if +// certain assumptions about how the task scheduler manages its threads +// holds true. +// The main assumption is: +// - when the threadpool is resized, the task scheduler either +// 1. destroys all worker threads and creates all new ones in the correct number, OR +// 2. never destroys a worker thread +// +// We make that assumption because we can't easily enumerate the worker threads of a task scheduler +// to assign nice sequential thread-indexes. We also do not get notified if a worker thread is destroyed, +// so we can't tell when a thread-index is no longer being used. +// We allocate thread-indexes as needed with a sequential global thread counter. +// +// Our simple thread-counting scheme falls apart if the task scheduler destroys some threads but +// continues to re-use other threads and the application repeatedly resizes the thread pool of the +// task scheduler. +// In order to prevent the thread-counter from exceeding the global max (BT_MAX_THREAD_COUNT), we +// wrap the thread counter back to 1. This should only happen if the worker threads have all been +// destroyed and re-created. +// +// BT_DETECT_BAD_THREAD_INDEX only works for Win32 right now, +// but could be adapted to work with pthreads +#define BT_DETECT_BAD_THREAD_INDEX 0 + +#if BT_DETECT_BAD_THREAD_INDEX + +typedef DWORD ThreadId_t; +const static ThreadId_t kInvalidThreadId = 0; +ThreadId_t gDebugThreadIds[ BT_MAX_THREAD_COUNT ]; + +static ThreadId_t getDebugThreadId() +{ + return GetCurrentThreadId(); +} + +#endif // #if BT_DETECT_BAD_THREAD_INDEX + + +// return a unique index per thread, main thread is 0, worker threads are in [1, BT_MAX_THREAD_COUNT) +unsigned int btGetCurrentThreadIndex() +{ + const unsigned int kNullIndex = ~0U; + THREAD_LOCAL_STATIC unsigned int sThreadIndex = kNullIndex; + if ( sThreadIndex == kNullIndex ) + { + sThreadIndex = gThreadCounter.getNext(); + btAssert( sThreadIndex < BT_MAX_THREAD_COUNT ); + } +#if BT_DETECT_BAD_THREAD_INDEX + if ( gBtTaskScheduler && sThreadIndex > 0 ) + { + ThreadId_t tid = getDebugThreadId(); + // if not set + if ( gDebugThreadIds[ sThreadIndex ] == kInvalidThreadId ) + { + // set it + gDebugThreadIds[ sThreadIndex ] = tid; + } + else + { + if ( gDebugThreadIds[ sThreadIndex ] != tid ) + { + // this could indicate the task scheduler is breaking our assumptions about + // how threads are managed when threadpool is resized + btAssert( !"there are 2 or more threads with the same thread-index!" ); + __debugbreak(); + } + } + } +#endif // #if BT_DETECT_BAD_THREAD_INDEX + return sThreadIndex; +} + +bool btIsMainThread() +{ + return btGetCurrentThreadIndex() == 0; +} + +void btResetThreadIndexCounter() +{ + // for when all current worker threads are destroyed + btAssert( btIsMainThread() ); + gThreadCounter.mCounter = 0; +} + +btITaskScheduler::btITaskScheduler( const char* name ) +{ + m_name = name; + m_savedThreadCounter = 0; + m_isActive = false; +} + +void btITaskScheduler::activate() +{ + // gThreadCounter is used to assign a thread-index to each worker thread in a task scheduler. + // The main thread is always thread-index 0, and worker threads are numbered from 1 to 63 (BT_MAX_THREAD_COUNT-1) + // The thread-indexes need to be unique amongst the threads that can be running simultaneously. + // Since only one task scheduler can be used at a time, it is OK for a pair of threads that belong to different + // task schedulers to share the same thread index because they can't be running at the same time. + // So each task scheduler needs to keep its own thread counter value + if ( !m_isActive ) + { + gThreadCounter.mCounter = m_savedThreadCounter; // restore saved thread counter + m_isActive = true; + } +} + +void btITaskScheduler::deactivate() +{ + if ( m_isActive ) + { + m_savedThreadCounter = gThreadCounter.mCounter; // save thread counter + m_isActive = false; + } +} + +void btPushThreadsAreRunning() +{ + gThreadsRunningCounterMutex.lock(); + gThreadsRunningCounter++; + gThreadsRunningCounterMutex.unlock(); +} + +void btPopThreadsAreRunning() +{ + gThreadsRunningCounterMutex.lock(); + gThreadsRunningCounter--; + gThreadsRunningCounterMutex.unlock(); +} + +bool btThreadsAreRunning() +{ + return gThreadsRunningCounter != 0; +} + + +void btSetTaskScheduler( btITaskScheduler* ts ) +{ + int threadId = btGetCurrentThreadIndex(); // make sure we call this on main thread at least once before any workers run + if ( threadId != 0 ) + { + btAssert( !"btSetTaskScheduler must be called from the main thread!" ); + return; + } + if ( gBtTaskScheduler ) + { + // deactivate old task scheduler + gBtTaskScheduler->deactivate(); + } + gBtTaskScheduler = ts; + if ( ts ) + { + // activate new task scheduler + ts->activate(); + } +} + + +btITaskScheduler* btGetTaskScheduler() +{ + return gBtTaskScheduler; +} + + +void btParallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) +{ +#if BT_THREADSAFE + +#if BT_DETECT_BAD_THREAD_INDEX + if ( !btThreadsAreRunning() ) + { + // clear out thread ids + for ( int i = 0; i < BT_MAX_THREAD_COUNT; ++i ) + { + gDebugThreadIds[ i ] = kInvalidThreadId; + } + } +#endif // #if BT_DETECT_BAD_THREAD_INDEX + + btAssert( gBtTaskScheduler != NULL ); // call btSetTaskScheduler() with a valid task scheduler first! + gBtTaskScheduler->parallelFor( iBegin, iEnd, grainSize, body ); + +#else // #if BT_THREADSAFE + + // non-parallel version of btParallelFor + btAssert( !"called btParallelFor in non-threadsafe build. enable BT_THREADSAFE" ); + body.forLoop( iBegin, iEnd ); + +#endif// #if BT_THREADSAFE +} + +btScalar btParallelSum( int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body ) +{ +#if BT_THREADSAFE + +#if BT_DETECT_BAD_THREAD_INDEX + if ( !btThreadsAreRunning() ) + { + // clear out thread ids + for ( int i = 0; i < BT_MAX_THREAD_COUNT; ++i ) + { + gDebugThreadIds[ i ] = kInvalidThreadId; + } + } +#endif // #if BT_DETECT_BAD_THREAD_INDEX + + btAssert( gBtTaskScheduler != NULL ); // call btSetTaskScheduler() with a valid task scheduler first! + return gBtTaskScheduler->parallelSum( iBegin, iEnd, grainSize, body ); + +#else // #if BT_THREADSAFE + + // non-parallel version of btParallelSum + btAssert( !"called btParallelFor in non-threadsafe build. enable BT_THREADSAFE" ); + return body.sumLoop( iBegin, iEnd ); + +#endif //#else // #if BT_THREADSAFE +} + + +/// +/// btTaskSchedulerSequential -- non-threaded implementation of task scheduler +/// (really just useful for testing performance of single threaded vs multi) +/// +class btTaskSchedulerSequential : public btITaskScheduler +{ +public: + btTaskSchedulerSequential() : btITaskScheduler( "Sequential" ) {} + virtual int getMaxNumThreads() const BT_OVERRIDE { return 1; } + virtual int getNumThreads() const BT_OVERRIDE { return 1; } + virtual void setNumThreads( int numThreads ) BT_OVERRIDE {} + virtual void parallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) BT_OVERRIDE + { + BT_PROFILE( "parallelFor_sequential" ); + body.forLoop( iBegin, iEnd ); + } + virtual btScalar parallelSum( int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body ) BT_OVERRIDE + { + BT_PROFILE( "parallelSum_sequential" ); + return body.sumLoop( iBegin, iEnd ); + } +}; + + +#if BT_USE_OPENMP && BT_THREADSAFE +/// +/// btTaskSchedulerOpenMP -- wrapper around OpenMP task scheduler +/// +class btTaskSchedulerOpenMP : public btITaskScheduler +{ + int m_numThreads; +public: + btTaskSchedulerOpenMP() : btITaskScheduler( "OpenMP" ) + { + m_numThreads = 0; + } + virtual int getMaxNumThreads() const BT_OVERRIDE + { + return omp_get_max_threads(); + } + virtual int getNumThreads() const BT_OVERRIDE + { + return m_numThreads; + } + virtual void setNumThreads( int numThreads ) BT_OVERRIDE + { + // With OpenMP, because it is a standard with various implementations, we can't + // know for sure if every implementation has the same behavior of destroying all + // previous threads when resizing the threadpool + m_numThreads = ( std::max )( 1, ( std::min )( int( BT_MAX_THREAD_COUNT ), numThreads ) ); + omp_set_num_threads( 1 ); // hopefully, all previous threads get destroyed here + omp_set_num_threads( m_numThreads ); + m_savedThreadCounter = 0; + if ( m_isActive ) + { + btResetThreadIndexCounter(); + } + } + virtual void parallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) BT_OVERRIDE + { + BT_PROFILE( "parallelFor_OpenMP" ); + btPushThreadsAreRunning(); +#pragma omp parallel for schedule( static, 1 ) + for ( int i = iBegin; i < iEnd; i += grainSize ) + { + BT_PROFILE( "OpenMP_forJob" ); + body.forLoop( i, ( std::min )( i + grainSize, iEnd ) ); + } + btPopThreadsAreRunning(); + } + virtual btScalar parallelSum( int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body ) BT_OVERRIDE + { + BT_PROFILE( "parallelFor_OpenMP" ); + btPushThreadsAreRunning(); + btScalar sum = btScalar( 0 ); +#pragma omp parallel for schedule( static, 1 ) reduction(+:sum) + for ( int i = iBegin; i < iEnd; i += grainSize ) + { + BT_PROFILE( "OpenMP_sumJob" ); + sum += body.sumLoop( i, ( std::min )( i + grainSize, iEnd ) ); + } + btPopThreadsAreRunning(); + return sum; + } +}; +#endif // #if BT_USE_OPENMP && BT_THREADSAFE + + +#if BT_USE_TBB && BT_THREADSAFE +/// +/// btTaskSchedulerTBB -- wrapper around Intel Threaded Building Blocks task scheduler +/// +class btTaskSchedulerTBB : public btITaskScheduler +{ + int m_numThreads; + tbb::task_scheduler_init* m_tbbSchedulerInit; + +public: + btTaskSchedulerTBB() : btITaskScheduler( "IntelTBB" ) + { + m_numThreads = 0; + m_tbbSchedulerInit = NULL; + } + ~btTaskSchedulerTBB() + { + if ( m_tbbSchedulerInit ) + { + delete m_tbbSchedulerInit; + m_tbbSchedulerInit = NULL; + } + } + + virtual int getMaxNumThreads() const BT_OVERRIDE + { + return tbb::task_scheduler_init::default_num_threads(); + } + virtual int getNumThreads() const BT_OVERRIDE + { + return m_numThreads; + } + virtual void setNumThreads( int numThreads ) BT_OVERRIDE + { + m_numThreads = ( std::max )( 1, ( std::min )( int(BT_MAX_THREAD_COUNT), numThreads ) ); + if ( m_tbbSchedulerInit ) + { + // destroys all previous threads + delete m_tbbSchedulerInit; + m_tbbSchedulerInit = NULL; + } + m_tbbSchedulerInit = new tbb::task_scheduler_init( m_numThreads ); + m_savedThreadCounter = 0; + if ( m_isActive ) + { + btResetThreadIndexCounter(); + } + } + struct ForBodyAdapter + { + const btIParallelForBody* mBody; + + ForBodyAdapter( const btIParallelForBody* body ) : mBody( body ) {} + void operator()( const tbb::blocked_range& range ) const + { + BT_PROFILE( "TBB_forJob" ); + mBody->forLoop( range.begin(), range.end() ); + } + }; + virtual void parallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) BT_OVERRIDE + { + BT_PROFILE( "parallelFor_TBB" ); + ForBodyAdapter tbbBody( &body ); + btPushThreadsAreRunning(); + tbb::parallel_for( tbb::blocked_range( iBegin, iEnd, grainSize ), + tbbBody, + tbb::simple_partitioner() + ); + btPopThreadsAreRunning(); + } + struct SumBodyAdapter + { + const btIParallelSumBody* mBody; + btScalar mSum; + + SumBodyAdapter( const btIParallelSumBody* body ) : mBody( body ), mSum( btScalar( 0 ) ) {} + SumBodyAdapter( const SumBodyAdapter& src, tbb::split ) : mBody( src.mBody ), mSum( btScalar( 0 ) ) {} + void join( const SumBodyAdapter& src ) { mSum += src.mSum; } + void operator()( const tbb::blocked_range& range ) + { + BT_PROFILE( "TBB_sumJob" ); + mSum += mBody->sumLoop( range.begin(), range.end() ); + } + }; + virtual btScalar parallelSum( int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body ) BT_OVERRIDE + { + BT_PROFILE( "parallelSum_TBB" ); + SumBodyAdapter tbbBody( &body ); + btPushThreadsAreRunning(); + tbb::parallel_deterministic_reduce( tbb::blocked_range( iBegin, iEnd, grainSize ), tbbBody ); + btPopThreadsAreRunning(); + return tbbBody.mSum; + } +}; +#endif // #if BT_USE_TBB && BT_THREADSAFE + + +#if BT_USE_PPL && BT_THREADSAFE +/// +/// btTaskSchedulerPPL -- wrapper around Microsoft Parallel Patterns Lib task scheduler +/// +class btTaskSchedulerPPL : public btITaskScheduler +{ + int m_numThreads; + concurrency::combinable m_sum; // for parallelSum +public: + btTaskSchedulerPPL() : btITaskScheduler( "PPL" ) + { + m_numThreads = 0; + } + virtual int getMaxNumThreads() const BT_OVERRIDE + { + return concurrency::GetProcessorCount(); + } + virtual int getNumThreads() const BT_OVERRIDE + { + return m_numThreads; + } + virtual void setNumThreads( int numThreads ) BT_OVERRIDE + { + // capping the thread count for PPL due to a thread-index issue + const int maxThreadCount = (std::min)(int(BT_MAX_THREAD_COUNT), 31); + m_numThreads = ( std::max )( 1, ( std::min )( maxThreadCount, numThreads ) ); + using namespace concurrency; + if ( CurrentScheduler::Id() != -1 ) + { + CurrentScheduler::Detach(); + } + SchedulerPolicy policy; + { + // PPL seems to destroy threads when threadpool is shrunk, but keeps reusing old threads + // force it to destroy old threads + policy.SetConcurrencyLimits( 1, 1 ); + CurrentScheduler::Create( policy ); + CurrentScheduler::Detach(); + } + policy.SetConcurrencyLimits( m_numThreads, m_numThreads ); + CurrentScheduler::Create( policy ); + m_savedThreadCounter = 0; + if ( m_isActive ) + { + btResetThreadIndexCounter(); + } + } + struct ForBodyAdapter + { + const btIParallelForBody* mBody; + int mGrainSize; + int mIndexEnd; + + ForBodyAdapter( const btIParallelForBody* body, int grainSize, int end ) : mBody( body ), mGrainSize( grainSize ), mIndexEnd( end ) {} + void operator()( int i ) const + { + BT_PROFILE( "PPL_forJob" ); + mBody->forLoop( i, ( std::min )( i + mGrainSize, mIndexEnd ) ); + } + }; + virtual void parallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) BT_OVERRIDE + { + BT_PROFILE( "parallelFor_PPL" ); + // PPL dispatch + ForBodyAdapter pplBody( &body, grainSize, iEnd ); + btPushThreadsAreRunning(); + // note: MSVC 2010 doesn't support partitioner args, so avoid them + concurrency::parallel_for( iBegin, + iEnd, + grainSize, + pplBody + ); + btPopThreadsAreRunning(); + } + struct SumBodyAdapter + { + const btIParallelSumBody* mBody; + concurrency::combinable* mSum; + int mGrainSize; + int mIndexEnd; + + SumBodyAdapter( const btIParallelSumBody* body, concurrency::combinable* sum, int grainSize, int end ) : mBody( body ), mSum(sum), mGrainSize( grainSize ), mIndexEnd( end ) {} + void operator()( int i ) const + { + BT_PROFILE( "PPL_sumJob" ); + mSum->local() += mBody->sumLoop( i, ( std::min )( i + mGrainSize, mIndexEnd ) ); + } + }; + static btScalar sumFunc( btScalar a, btScalar b ) { return a + b; } + virtual btScalar parallelSum( int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body ) BT_OVERRIDE + { + BT_PROFILE( "parallelSum_PPL" ); + m_sum.clear(); + SumBodyAdapter pplBody( &body, &m_sum, grainSize, iEnd ); + btPushThreadsAreRunning(); + // note: MSVC 2010 doesn't support partitioner args, so avoid them + concurrency::parallel_for( iBegin, + iEnd, + grainSize, + pplBody + ); + btPopThreadsAreRunning(); + return m_sum.combine( sumFunc ); + } +}; +#endif // #if BT_USE_PPL && BT_THREADSAFE + + +// create a non-threaded task scheduler (always available) +btITaskScheduler* btGetSequentialTaskScheduler() +{ + static btTaskSchedulerSequential sTaskScheduler; + return &sTaskScheduler; +} + + +// create an OpenMP task scheduler (if available, otherwise returns null) +btITaskScheduler* btGetOpenMPTaskScheduler() +{ +#if BT_USE_OPENMP && BT_THREADSAFE + static btTaskSchedulerOpenMP sTaskScheduler; + return &sTaskScheduler; +#else + return NULL; +#endif +} + + +// create an Intel TBB task scheduler (if available, otherwise returns null) +btITaskScheduler* btGetTBBTaskScheduler() +{ +#if BT_USE_TBB && BT_THREADSAFE + static btTaskSchedulerTBB sTaskScheduler; + return &sTaskScheduler; +#else + return NULL; +#endif +} + + +// create a PPL task scheduler (if available, otherwise returns null) +btITaskScheduler* btGetPPLTaskScheduler() +{ +#if BT_USE_PPL && BT_THREADSAFE + static btTaskSchedulerPPL sTaskScheduler; + return &sTaskScheduler; +#else + return NULL; +#endif +} + diff --git a/extern/bullet/src/LinearMath/btThreads.h b/extern/bullet/src/LinearMath/btThreads.h new file mode 100644 index 000000000000..ecd5a19cfa90 --- /dev/null +++ b/extern/bullet/src/LinearMath/btThreads.h @@ -0,0 +1,180 @@ +/* +Copyright (c) 2003-2014 Erwin Coumans http://bullet.googlecode.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef BT_THREADS_H +#define BT_THREADS_H + +#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE + +#if defined (_MSC_VER) && _MSC_VER >= 1600 +// give us a compile error if any signatures of overriden methods is changed +#define BT_OVERRIDE override +#endif + +#ifndef BT_OVERRIDE +#define BT_OVERRIDE +#endif + +const unsigned int BT_MAX_THREAD_COUNT = 64; // only if BT_THREADSAFE is 1 + +// for internal use only +bool btIsMainThread(); +bool btThreadsAreRunning(); +unsigned int btGetCurrentThreadIndex(); +void btResetThreadIndexCounter(); // notify that all worker threads have been destroyed + +/// +/// btSpinMutex -- lightweight spin-mutex implemented with atomic ops, never puts +/// a thread to sleep because it is designed to be used with a task scheduler +/// which has one thread per core and the threads don't sleep until they +/// run out of tasks. Not good for general purpose use. +/// +class btSpinMutex +{ + int mLock; + +public: + btSpinMutex() + { + mLock = 0; + } + void lock(); + void unlock(); + bool tryLock(); +}; + + +// +// NOTE: btMutex* is for internal Bullet use only +// +// If BT_THREADSAFE is undefined or 0, should optimize away to nothing. +// This is good because for the single-threaded build of Bullet, any calls +// to these functions will be optimized out. +// +// However, for users of the multi-threaded build of Bullet this is kind +// of bad because if you call any of these functions from external code +// (where BT_THREADSAFE is undefined) you will get unexpected race conditions. +// +SIMD_FORCE_INLINE void btMutexLock( btSpinMutex* mutex ) +{ +#if BT_THREADSAFE + mutex->lock(); +#else + (void)mutex; +#endif // #if BT_THREADSAFE +} + +SIMD_FORCE_INLINE void btMutexUnlock( btSpinMutex* mutex ) +{ +#if BT_THREADSAFE + mutex->unlock(); +#else + (void)mutex; +#endif // #if BT_THREADSAFE +} + +SIMD_FORCE_INLINE bool btMutexTryLock( btSpinMutex* mutex ) +{ +#if BT_THREADSAFE + return mutex->tryLock(); +#else + (void)mutex; + return true; +#endif // #if BT_THREADSAFE +} + + +// +// btIParallelForBody -- subclass this to express work that can be done in parallel +// +class btIParallelForBody +{ +public: + virtual ~btIParallelForBody() {} + virtual void forLoop( int iBegin, int iEnd ) const = 0; +}; + +// +// btIParallelSumBody -- subclass this to express work that can be done in parallel +// and produces a sum over all loop elements +// +class btIParallelSumBody +{ +public: + virtual ~btIParallelSumBody() {} + virtual btScalar sumLoop( int iBegin, int iEnd ) const = 0; +}; + +// +// btITaskScheduler -- subclass this to implement a task scheduler that can dispatch work to +// worker threads +// +class btITaskScheduler +{ +public: + btITaskScheduler( const char* name ); + virtual ~btITaskScheduler() {} + const char* getName() const { return m_name; } + + virtual int getMaxNumThreads() const = 0; + virtual int getNumThreads() const = 0; + virtual void setNumThreads( int numThreads ) = 0; + virtual void parallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) = 0; + virtual btScalar parallelSum( int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body ) = 0; + virtual void sleepWorkerThreadsHint() {} // hint the task scheduler that we may not be using these threads for a little while + + // internal use only + virtual void activate(); + virtual void deactivate(); + +protected: + const char* m_name; + unsigned int m_savedThreadCounter; + bool m_isActive; +}; + +// set the task scheduler to use for all calls to btParallelFor() +// NOTE: you must set this prior to using any of the multi-threaded "Mt" classes +void btSetTaskScheduler( btITaskScheduler* ts ); + +// get the current task scheduler +btITaskScheduler* btGetTaskScheduler(); + +// get non-threaded task scheduler (always available) +btITaskScheduler* btGetSequentialTaskScheduler(); + +// create a default task scheduler (Win32 or pthreads based) +btITaskScheduler* btCreateDefaultTaskScheduler(); + +// get OpenMP task scheduler (if available, otherwise returns null) +btITaskScheduler* btGetOpenMPTaskScheduler(); + +// get Intel TBB task scheduler (if available, otherwise returns null) +btITaskScheduler* btGetTBBTaskScheduler(); + +// get PPL task scheduler (if available, otherwise returns null) +btITaskScheduler* btGetPPLTaskScheduler(); + +// btParallelFor -- call this to dispatch work like a for-loop +// (iterations may be done out of order, so no dependencies are allowed) +void btParallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ); + +// btParallelSum -- call this to dispatch work like a for-loop, returns the sum of all iterations +// (iterations may be done out of order, so no dependencies are allowed) +btScalar btParallelSum( int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body ); + + +#endif diff --git a/extern/bullet2/src/LinearMath/btTransform.h b/extern/bullet/src/LinearMath/btTransform.h similarity index 100% rename from extern/bullet2/src/LinearMath/btTransform.h rename to extern/bullet/src/LinearMath/btTransform.h diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet/src/LinearMath/btTransformUtil.h similarity index 94% rename from extern/bullet2/src/LinearMath/btTransformUtil.h rename to extern/bullet/src/LinearMath/btTransformUtil.h index 2303c2742753..182cc43fab90 100644 --- a/extern/bullet2/src/LinearMath/btTransformUtil.h +++ b/extern/bullet/src/LinearMath/btTransformUtil.h @@ -47,13 +47,19 @@ class btTransformUtil #ifdef QUATERNION_DERIVATIVE btQuaternion predictedOrn = curTrans.getRotation(); predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5)); - predictedOrn.normalize(); + predictedOrn.safeNormalize(); #else //Exponential map //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia btVector3 axis; - btScalar fAngle = angvel.length(); + btScalar fAngle2 = angvel.length2(); + btScalar fAngle = 0; + if (fAngle2>SIMD_EPSILON) + { + fAngle = btSqrt(fAngle2); + } + //limit the angular motion if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD) { @@ -74,9 +80,16 @@ class btTransformUtil btQuaternion orn0 = curTrans.getRotation(); btQuaternion predictedOrn = dorn * orn0; - predictedOrn.normalize(); + predictedOrn.safeNormalize(); #endif - predictedTransform.setRotation(predictedOrn); + if (predictedOrn.length2()>SIMD_EPSILON) + { + predictedTransform.setRotation(predictedOrn); + } + else + { + predictedTransform.setBasis(curTrans.getBasis()); + } } static void calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel) diff --git a/extern/bullet2/src/LinearMath/btVector3.cpp b/extern/bullet/src/LinearMath/btVector3.cpp similarity index 100% rename from extern/bullet2/src/LinearMath/btVector3.cpp rename to extern/bullet/src/LinearMath/btVector3.cpp diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet/src/LinearMath/btVector3.h similarity index 97% rename from extern/bullet2/src/LinearMath/btVector3.h rename to extern/bullet/src/LinearMath/btVector3.h index 839b19c1449d..cfc9354ecee1 100644 --- a/extern/bullet2/src/LinearMath/btVector3.h +++ b/extern/bullet/src/LinearMath/btVector3.h @@ -267,10 +267,20 @@ ATTRIBUTE_ALIGNED16(class) btVector3 /**@brief Return the norm (length) of the vector */ SIMD_FORCE_INLINE btScalar norm() const - { + { return length(); } + /**@brief Return the norm (length) of the vector */ + SIMD_FORCE_INLINE btScalar safeNorm() const + { + btScalar d = length2(); + //workaround for some clang/gcc issue of sqrtf(tiny number) = -INF + if (d>SIMD_EPSILON) + return btSqrt(d); + return btScalar(0); + } + /**@brief Return the distance squared between the ends of this and another vector * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const; @@ -281,14 +291,16 @@ ATTRIBUTE_ALIGNED16(class) btVector3 SIMD_FORCE_INLINE btVector3& safeNormalize() { - btVector3 absVec = this->absolute(); - int maxIndex = absVec.maxAxis(); - if (absVec[maxIndex]>0) + btScalar l2 = length2(); + //triNormal.normalize(); + if (l2 >= SIMD_EPSILON*SIMD_EPSILON) + { + (*this) /= btSqrt(l2); + } + else { - *this /= absVec[maxIndex]; - return *this /= length(); + setValue(1, 0, 0); } - setValue(1,0,0); return *this; } @@ -356,7 +368,7 @@ ATTRIBUTE_ALIGNED16(class) btVector3 return btAcos(dot(v) / s); } - /**@brief Return a vector will the absolute values of each element */ + /**@brief Return a vector with the absolute values of each element */ SIMD_FORCE_INLINE btVector3 absolute() const { @@ -693,7 +705,9 @@ ATTRIBUTE_ALIGNED16(class) btVector3 SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const; - SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn); + SIMD_FORCE_INLINE void deSerialize(const struct btVector3DoubleData& dataIn); + + SIMD_FORCE_INLINE void deSerialize(const struct btVector3FloatData& dataIn); SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData& dataOut) const; @@ -1147,7 +1161,6 @@ class btVector4 : public btVector3 if (m_floats[3] > maxVal) { maxIndex = 3; - maxVal = m_floats[3]; } return maxIndex; @@ -1176,7 +1189,6 @@ class btVector4 : public btVector3 if (m_floats[3] < minVal) { minIndex = 3; - minVal = m_floats[3]; } return minIndex; @@ -1344,10 +1356,18 @@ SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const dataOut.m_floats[i] = m_floats[i]; } -SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn) + +SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3FloatData& dataIn) +{ + for (int i = 0; i<4; i++) + m_floats[i] = (btScalar)dataIn.m_floats[i]; +} + + +SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3DoubleData& dataIn) { for (int i=0;i<4;i++) - m_floats[i] = dataIn.m_floats[i]; + m_floats[i] = (btScalar)dataIn.m_floats[i]; } #endif //BT_VECTOR3_H diff --git a/extern/bullet/src/LinearMath/premake4.lua b/extern/bullet/src/LinearMath/premake4.lua new file mode 100644 index 000000000000..3765811a943e --- /dev/null +++ b/extern/bullet/src/LinearMath/premake4.lua @@ -0,0 +1,15 @@ + project "LinearMath" + + kind "StaticLib" + if os.is("Linux") then + buildoptions{"-fPIC"} + end + includedirs { + "..", + } + files { + "*.cpp", + "*.h", + "TaskScheduler/*.cpp", + "TaskScheduler/*.h" + } diff --git a/extern/bullet2/src/btBulletCollisionCommon.h b/extern/bullet/src/btBulletCollisionCommon.h similarity index 97% rename from extern/bullet2/src/btBulletCollisionCommon.h rename to extern/bullet/src/btBulletCollisionCommon.h index af981b5d36dc..948e02eb4cd6 100644 --- a/extern/bullet2/src/btBulletCollisionCommon.h +++ b/extern/bullet/src/btBulletCollisionCommon.h @@ -52,7 +52,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" #include "BulletCollision/BroadphaseCollision/btAxisSweep3.h" -#include "BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h" #include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h" ///Math library & Utils diff --git a/extern/bullet2/src/btBulletDynamicsCommon.h b/extern/bullet/src/btBulletDynamicsCommon.h similarity index 100% rename from extern/bullet2/src/btBulletDynamicsCommon.h rename to extern/bullet/src/btBulletDynamicsCommon.h diff --git a/extern/bullet/src/clew/clew.c b/extern/bullet/src/clew/clew.c new file mode 100644 index 000000000000..5afc42a48553 --- /dev/null +++ b/extern/bullet/src/clew/clew.c @@ -0,0 +1,312 @@ +////////////////////////////////////////////////////////////////////////// +// Copyright (c) 2009 Organic Vectory B.V. +// Written by George van Venrooij +// +// Distributed under the Boost Software License, Version 1.0. +// (See accompanying file license.txt) +////////////////////////////////////////////////////////////////////////// + +#include "clew.h" + +#ifdef _WIN32 + #define WIN32_LEAN_AND_MEAN + #define VC_EXTRALEAN + #include + + typedef HMODULE CLEW_DYNLIB_HANDLE; + + #define CLEW_DYNLIB_OPEN LoadLibraryA + #define CLEW_DYNLIB_CLOSE FreeLibrary + #define CLEW_DYNLIB_IMPORT GetProcAddress +#else + #include + + typedef void* CLEW_DYNLIB_HANDLE; + + #define CLEW_DYNLIB_OPEN(path) dlopen(path, RTLD_NOW | RTLD_GLOBAL) + #define CLEW_DYNLIB_CLOSE dlclose + #define CLEW_DYNLIB_IMPORT dlsym +#endif + +#include + +//! \brief module handle +static CLEW_DYNLIB_HANDLE module = NULL; + +// Variables holding function entry points +PFNCLGETPLATFORMIDS __clewGetPlatformIDs = NULL; +PFNCLGETPLATFORMINFO __clewGetPlatformInfo = NULL; +PFNCLGETDEVICEIDS __clewGetDeviceIDs = NULL; +PFNCLGETDEVICEINFO __clewGetDeviceInfo = NULL; +PFNCLCREATECONTEXT __clewCreateContext = NULL; +PFNCLCREATECONTEXTFROMTYPE __clewCreateContextFromType = NULL; +PFNCLRETAINCONTEXT __clewRetainContext = NULL; +PFNCLRELEASECONTEXT __clewReleaseContext = NULL; +PFNCLGETCONTEXTINFO __clewGetContextInfo = NULL; +PFNCLCREATECOMMANDQUEUE __clewCreateCommandQueue = NULL; +PFNCLRETAINCOMMANDQUEUE __clewRetainCommandQueue = NULL; +PFNCLRELEASECOMMANDQUEUE __clewReleaseCommandQueue = NULL; +PFNCLGETCOMMANDQUEUEINFO __clewGetCommandQueueInfo = NULL; +#ifdef CL_USE_DEPRECATED_OPENCL_1_0_APIS +PFNCLSETCOMMANDQUEUEPROPERTY __clewSetCommandQueueProperty = NULL; +#endif +PFNCLCREATEBUFFER __clewCreateBuffer = NULL; +PFNCLCREATESUBBUFFER __clewCreateSubBuffer = NULL; +PFNCLCREATEIMAGE2D __clewCreateImage2D = NULL; +PFNCLCREATEIMAGE3D __clewCreateImage3D = NULL; +PFNCLRETAINMEMOBJECT __clewRetainMemObject = NULL; +PFNCLRELEASEMEMOBJECT __clewReleaseMemObject = NULL; +PFNCLGETSUPPORTEDIMAGEFORMATS __clewGetSupportedImageFormats = NULL; +PFNCLGETMEMOBJECTINFO __clewGetMemObjectInfo = NULL; +PFNCLGETIMAGEINFO __clewGetImageInfo = NULL; +PFNCLSETMEMOBJECTDESTRUCTORCALLBACK __clewSetMemObjectDestructorCallback = NULL; +PFNCLCREATESAMPLER __clewCreateSampler = NULL; +PFNCLRETAINSAMPLER __clewRetainSampler = NULL; +PFNCLRELEASESAMPLER __clewReleaseSampler = NULL; +PFNCLGETSAMPLERINFO __clewGetSamplerInfo = NULL; +PFNCLCREATEPROGRAMWITHSOURCE __clewCreateProgramWithSource = NULL; +PFNCLCREATEPROGRAMWITHBINARY __clewCreateProgramWithBinary = NULL; +PFNCLRETAINPROGRAM __clewRetainProgram = NULL; +PFNCLRELEASEPROGRAM __clewReleaseProgram = NULL; +PFNCLBUILDPROGRAM __clewBuildProgram = NULL; +PFNCLUNLOADCOMPILER __clewUnloadCompiler = NULL; +PFNCLGETPROGRAMINFO __clewGetProgramInfo = NULL; +PFNCLGETPROGRAMBUILDINFO __clewGetProgramBuildInfo = NULL; +PFNCLCREATEKERNEL __clewCreateKernel = NULL; +PFNCLCREATEKERNELSINPROGRAM __clewCreateKernelsInProgram = NULL; +PFNCLRETAINKERNEL __clewRetainKernel = NULL; +PFNCLRELEASEKERNEL __clewReleaseKernel = NULL; +PFNCLSETKERNELARG __clewSetKernelArg = NULL; +PFNCLGETKERNELINFO __clewGetKernelInfo = NULL; +PFNCLGETKERNELWORKGROUPINFO __clewGetKernelWorkGroupInfo = NULL; +PFNCLWAITFOREVENTS __clewWaitForEvents = NULL; +PFNCLGETEVENTINFO __clewGetEventInfo = NULL; +PFNCLCREATEUSEREVENT __clewCreateUserEvent = NULL; +PFNCLRETAINEVENT __clewRetainEvent = NULL; +PFNCLRELEASEEVENT __clewReleaseEvent = NULL; +PFNCLSETUSEREVENTSTATUS __clewSetUserEventStatus = NULL; +PFNCLSETEVENTCALLBACK __clewSetEventCallback = NULL; +PFNCLGETEVENTPROFILINGINFO __clewGetEventProfilingInfo = NULL; +PFNCLFLUSH __clewFlush = NULL; +PFNCLFINISH __clewFinish = NULL; +PFNCLENQUEUEREADBUFFER __clewEnqueueReadBuffer = NULL; +PFNCLENQUEUEREADBUFFERRECT __clewEnqueueReadBufferRect = NULL; +PFNCLENQUEUEWRITEBUFFER __clewEnqueueWriteBuffer = NULL; +PFNCLENQUEUEWRITEBUFFERRECT __clewEnqueueWriteBufferRect = NULL; +PFNCLENQUEUECOPYBUFFER __clewEnqueueCopyBuffer = NULL; +PFNCLENQUEUEREADIMAGE __clewEnqueueReadImage = NULL; +PFNCLENQUEUEWRITEIMAGE __clewEnqueueWriteImage = NULL; +PFNCLENQUEUECOPYIMAGE __clewEnqueueCopyImage = NULL; +PFNCLENQUEUECOPYBUFFERRECT __clewEnqueueCopyBufferRect = NULL; +PFNCLENQUEUECOPYIMAGETOBUFFER __clewEnqueueCopyImageToBuffer = NULL; +PFNCLENQUEUECOPYBUFFERTOIMAGE __clewEnqueueCopyBufferToImage = NULL; +PFNCLENQUEUEMAPBUFFER __clewEnqueueMapBuffer = NULL; +PFNCLENQUEUEMAPIMAGE __clewEnqueueMapImage = NULL; +PFNCLENQUEUEUNMAPMEMOBJECT __clewEnqueueUnmapMemObject = NULL; +PFNCLENQUEUENDRANGEKERNEL __clewEnqueueNDRangeKernel = NULL; +PFNCLENQUEUETASK __clewEnqueueTask = NULL; +PFNCLENQUEUENATIVEKERNEL __clewEnqueueNativeKernel = NULL; +PFNCLENQUEUEMARKER __clewEnqueueMarker = NULL; +PFNCLENQUEUEWAITFOREVENTS __clewEnqueueWaitForEvents = NULL; +PFNCLENQUEUEBARRIER __clewEnqueueBarrier = NULL; +PFNCLGETEXTENSIONFUNCTIONADDRESS __clewGetExtensionFunctionAddress = NULL; + + +void clewExit(void) +{ + if (module != NULL) + { + // Ignore errors + CLEW_DYNLIB_CLOSE(module); + module = NULL; + } +} + +int clewInit(const char* path) +{ + int error = 0; + + // Check if already initialized + if (module != NULL) + { + return CLEW_SUCCESS; + } + + // Load library + module = CLEW_DYNLIB_OPEN(path); + + // Check for errors + if (module == NULL) + { + return CLEW_ERROR_OPEN_FAILED; + } + + // Set unloading + error = atexit(clewExit); + + if (error) + { + // Failure queuing atexit, shutdown with error + CLEW_DYNLIB_CLOSE(module); + module = NULL; + + return CLEW_ERROR_ATEXIT_FAILED; + } + + // Determine function entry-points + __clewGetPlatformIDs = (PFNCLGETPLATFORMIDS )CLEW_DYNLIB_IMPORT(module, "clGetPlatformIDs"); + __clewGetPlatformInfo = (PFNCLGETPLATFORMINFO )CLEW_DYNLIB_IMPORT(module, "clGetPlatformInfo"); + __clewGetDeviceIDs = (PFNCLGETDEVICEIDS )CLEW_DYNLIB_IMPORT(module, "clGetDeviceIDs"); + __clewGetDeviceInfo = (PFNCLGETDEVICEINFO )CLEW_DYNLIB_IMPORT(module, "clGetDeviceInfo"); + __clewCreateContext = (PFNCLCREATECONTEXT )CLEW_DYNLIB_IMPORT(module, "clCreateContext"); + __clewCreateContextFromType = (PFNCLCREATECONTEXTFROMTYPE )CLEW_DYNLIB_IMPORT(module, "clCreateContextFromType"); + __clewRetainContext = (PFNCLRETAINCONTEXT )CLEW_DYNLIB_IMPORT(module, "clRetainContext"); + __clewReleaseContext = (PFNCLRELEASECONTEXT )CLEW_DYNLIB_IMPORT(module, "clReleaseContext"); + __clewGetContextInfo = (PFNCLGETCONTEXTINFO )CLEW_DYNLIB_IMPORT(module, "clGetContextInfo"); + __clewCreateCommandQueue = (PFNCLCREATECOMMANDQUEUE )CLEW_DYNLIB_IMPORT(module, "clCreateCommandQueue"); + __clewRetainCommandQueue = (PFNCLRETAINCOMMANDQUEUE )CLEW_DYNLIB_IMPORT(module, "clRetainCommandQueue"); + __clewReleaseCommandQueue = (PFNCLRELEASECOMMANDQUEUE )CLEW_DYNLIB_IMPORT(module, "clReleaseCommandQueue"); + __clewGetCommandQueueInfo = (PFNCLGETCOMMANDQUEUEINFO )CLEW_DYNLIB_IMPORT(module, "clGetCommandQueueInfo"); +#ifdef CL_USE_DEPRECATED_OPENCL_1_0_APIS + __clewSetCommandQueueProperty = (PFNCLSETCOMMANDQUEUEPROPERTY )CLEW_DYNLIB_IMPORT(module, "clSetCommandQueueProperty"); +#endif + __clewCreateBuffer = (PFNCLCREATEBUFFER )CLEW_DYNLIB_IMPORT(module, "clCreateBuffer"); + __clewCreateSubBuffer = (PFNCLCREATESUBBUFFER )CLEW_DYNLIB_IMPORT(module, "clCreateBuffer"); + __clewCreateImage2D = (PFNCLCREATEIMAGE2D )CLEW_DYNLIB_IMPORT(module, "clCreateImage2D"); + __clewCreateImage3D = (PFNCLCREATEIMAGE3D )CLEW_DYNLIB_IMPORT(module, "clCreateImage3D"); + __clewRetainMemObject = (PFNCLRETAINMEMOBJECT )CLEW_DYNLIB_IMPORT(module, "clRetainMemObject"); + __clewReleaseMemObject = (PFNCLRELEASEMEMOBJECT )CLEW_DYNLIB_IMPORT(module, "clReleaseMemObject"); + __clewGetSupportedImageFormats = (PFNCLGETSUPPORTEDIMAGEFORMATS )CLEW_DYNLIB_IMPORT(module, "clGetSupportedImageFormats"); + __clewGetMemObjectInfo = (PFNCLGETMEMOBJECTINFO )CLEW_DYNLIB_IMPORT(module, "clGetMemObjectInfo"); + __clewGetImageInfo = (PFNCLGETIMAGEINFO )CLEW_DYNLIB_IMPORT(module, "clGetImageInfo"); + __clewSetMemObjectDestructorCallback = (PFNCLSETMEMOBJECTDESTRUCTORCALLBACK)CLEW_DYNLIB_IMPORT(module, "clSetMemObjectDestructorCallback"); + __clewCreateSampler = (PFNCLCREATESAMPLER )CLEW_DYNLIB_IMPORT(module, "clCreateSampler"); + __clewRetainSampler = (PFNCLRETAINSAMPLER )CLEW_DYNLIB_IMPORT(module, "clRetainSampler"); + __clewReleaseSampler = (PFNCLRELEASESAMPLER )CLEW_DYNLIB_IMPORT(module, "clReleaseSampler"); + __clewGetSamplerInfo = (PFNCLGETSAMPLERINFO )CLEW_DYNLIB_IMPORT(module, "clGetSamplerInfo"); + __clewCreateProgramWithSource = (PFNCLCREATEPROGRAMWITHSOURCE )CLEW_DYNLIB_IMPORT(module, "clCreateProgramWithSource"); + __clewCreateProgramWithBinary = (PFNCLCREATEPROGRAMWITHBINARY )CLEW_DYNLIB_IMPORT(module, "clCreateProgramWithBinary"); + __clewRetainProgram = (PFNCLRETAINPROGRAM )CLEW_DYNLIB_IMPORT(module, "clRetainProgram"); + __clewReleaseProgram = (PFNCLRELEASEPROGRAM )CLEW_DYNLIB_IMPORT(module, "clReleaseProgram"); + __clewBuildProgram = (PFNCLBUILDPROGRAM )CLEW_DYNLIB_IMPORT(module, "clBuildProgram"); + __clewUnloadCompiler = (PFNCLUNLOADCOMPILER )CLEW_DYNLIB_IMPORT(module, "clUnloadCompiler"); + __clewGetProgramInfo = (PFNCLGETPROGRAMINFO )CLEW_DYNLIB_IMPORT(module, "clGetProgramInfo"); + __clewGetProgramBuildInfo = (PFNCLGETPROGRAMBUILDINFO )CLEW_DYNLIB_IMPORT(module, "clGetProgramBuildInfo"); + __clewCreateKernel = (PFNCLCREATEKERNEL )CLEW_DYNLIB_IMPORT(module, "clCreateKernel"); + __clewCreateKernelsInProgram = (PFNCLCREATEKERNELSINPROGRAM )CLEW_DYNLIB_IMPORT(module, "clCreateKernelsInProgram"); + __clewRetainKernel = (PFNCLRETAINKERNEL )CLEW_DYNLIB_IMPORT(module, "clRetainKernel"); + __clewReleaseKernel = (PFNCLRELEASEKERNEL )CLEW_DYNLIB_IMPORT(module, "clReleaseKernel"); + __clewSetKernelArg = (PFNCLSETKERNELARG )CLEW_DYNLIB_IMPORT(module, "clSetKernelArg"); + __clewGetKernelInfo = (PFNCLGETKERNELINFO )CLEW_DYNLIB_IMPORT(module, "clGetKernelInfo"); + __clewGetKernelWorkGroupInfo = (PFNCLGETKERNELWORKGROUPINFO )CLEW_DYNLIB_IMPORT(module, "clGetKernelWorkGroupInfo"); + __clewWaitForEvents = (PFNCLWAITFOREVENTS )CLEW_DYNLIB_IMPORT(module, "clWaitForEvents"); + __clewGetEventInfo = (PFNCLGETEVENTINFO )CLEW_DYNLIB_IMPORT(module, "clGetEventInfo"); + __clewCreateUserEvent = (PFNCLCREATEUSEREVENT )CLEW_DYNLIB_IMPORT(module, "clCreateUserEvent"); + __clewRetainEvent = (PFNCLRETAINEVENT )CLEW_DYNLIB_IMPORT(module, "clRetainEvent"); + __clewReleaseEvent = (PFNCLRELEASEEVENT )CLEW_DYNLIB_IMPORT(module, "clReleaseEvent"); + __clewSetUserEventStatus = (PFNCLSETUSEREVENTSTATUS )CLEW_DYNLIB_IMPORT(module, "clSetUserEventStatus"); + __clewSetEventCallback = (PFNCLSETEVENTCALLBACK )CLEW_DYNLIB_IMPORT(module, "clSetEventCallback"); + __clewGetEventProfilingInfo = (PFNCLGETEVENTPROFILINGINFO )CLEW_DYNLIB_IMPORT(module, "clGetEventProfilingInfo"); + __clewFlush = (PFNCLFLUSH )CLEW_DYNLIB_IMPORT(module, "clFlush"); + __clewFinish = (PFNCLFINISH )CLEW_DYNLIB_IMPORT(module, "clFinish"); + __clewEnqueueReadBuffer = (PFNCLENQUEUEREADBUFFER )CLEW_DYNLIB_IMPORT(module, "clEnqueueReadBuffer"); + __clewEnqueueReadBufferRect = (PFNCLENQUEUEREADBUFFERRECT )CLEW_DYNLIB_IMPORT(module, "clEnqueueReadBufferRect"); + __clewEnqueueWriteBuffer = (PFNCLENQUEUEWRITEBUFFER )CLEW_DYNLIB_IMPORT(module, "clEnqueueWriteBuffer"); + __clewEnqueueWriteBufferRect = (PFNCLENQUEUEWRITEBUFFERRECT )CLEW_DYNLIB_IMPORT(module, "clEnqueueWriteBufferRect"); + __clewEnqueueCopyBuffer = (PFNCLENQUEUECOPYBUFFER )CLEW_DYNLIB_IMPORT(module, "clEnqueueCopyBuffer"); + __clewEnqueueCopyBufferRect = (PFNCLENQUEUECOPYBUFFERRECT )CLEW_DYNLIB_IMPORT(module, "clEnqueueCopyBufferRect"); + __clewEnqueueReadImage = (PFNCLENQUEUEREADIMAGE )CLEW_DYNLIB_IMPORT(module, "clEnqueueReadImage"); + __clewEnqueueWriteImage = (PFNCLENQUEUEWRITEIMAGE )CLEW_DYNLIB_IMPORT(module, "clEnqueueWriteImage"); + __clewEnqueueCopyImage = (PFNCLENQUEUECOPYIMAGE )CLEW_DYNLIB_IMPORT(module, "clEnqueueCopyImage"); + __clewEnqueueCopyImageToBuffer = (PFNCLENQUEUECOPYIMAGETOBUFFER )CLEW_DYNLIB_IMPORT(module, "clEnqueueCopyImageToBuffer"); + __clewEnqueueCopyBufferToImage = (PFNCLENQUEUECOPYBUFFERTOIMAGE )CLEW_DYNLIB_IMPORT(module, "clEnqueueCopyBufferToImage"); + __clewEnqueueMapBuffer = (PFNCLENQUEUEMAPBUFFER )CLEW_DYNLIB_IMPORT(module, "clEnqueueMapBuffer"); + __clewEnqueueMapImage = (PFNCLENQUEUEMAPIMAGE )CLEW_DYNLIB_IMPORT(module, "clEnqueueMapImage"); + __clewEnqueueUnmapMemObject = (PFNCLENQUEUEUNMAPMEMOBJECT )CLEW_DYNLIB_IMPORT(module, "clEnqueueUnmapMemObject"); + __clewEnqueueNDRangeKernel = (PFNCLENQUEUENDRANGEKERNEL )CLEW_DYNLIB_IMPORT(module, "clEnqueueNDRangeKernel"); + __clewEnqueueTask = (PFNCLENQUEUETASK )CLEW_DYNLIB_IMPORT(module, "clEnqueueTask"); + __clewEnqueueNativeKernel = (PFNCLENQUEUENATIVEKERNEL )CLEW_DYNLIB_IMPORT(module, "clEnqueueNativeKernel"); + __clewEnqueueMarker = (PFNCLENQUEUEMARKER )CLEW_DYNLIB_IMPORT(module, "clEnqueueMarker"); + __clewEnqueueWaitForEvents = (PFNCLENQUEUEWAITFOREVENTS )CLEW_DYNLIB_IMPORT(module, "clEnqueueWaitForEvents"); + __clewEnqueueBarrier = (PFNCLENQUEUEBARRIER )CLEW_DYNLIB_IMPORT(module, "clEnqueueBarrier"); + __clewGetExtensionFunctionAddress = (PFNCLGETEXTENSIONFUNCTIONADDRESS )CLEW_DYNLIB_IMPORT(module, "clGetExtensionFunctionAddress"); + + return CLEW_SUCCESS; +} + +const char* clewErrorString(cl_int error) +{ + static const char* strings[] = + { + // Error Codes + "CL_SUCCESS" // 0 + , "CL_DEVICE_NOT_FOUND" // -1 + , "CL_DEVICE_NOT_AVAILABLE" // -2 + , "CL_COMPILER_NOT_AVAILABLE" // -3 + , "CL_MEM_OBJECT_ALLOCATION_FAILURE" // -4 + , "CL_OUT_OF_RESOURCES" // -5 + , "CL_OUT_OF_HOST_MEMORY" // -6 + , "CL_PROFILING_INFO_NOT_AVAILABLE" // -7 + , "CL_MEM_COPY_OVERLAP" // -8 + , "CL_IMAGE_FORMAT_MISMATCH" // -9 + , "CL_IMAGE_FORMAT_NOT_SUPPORTED" // -10 + , "CL_BUILD_PROGRAM_FAILURE" // -11 + , "CL_MAP_FAILURE" // -12 + + , "" // -13 + , "" // -14 + , "" // -15 + , "" // -16 + , "" // -17 + , "" // -18 + , "" // -19 + + , "" // -20 + , "" // -21 + , "" // -22 + , "" // -23 + , "" // -24 + , "" // -25 + , "" // -26 + , "" // -27 + , "" // -28 + , "" // -29 + + , "CL_INVALID_VALUE" // -30 + , "CL_INVALID_DEVICE_TYPE" // -31 + , "CL_INVALID_PLATFORM" // -32 + , "CL_INVALID_DEVICE" // -33 + , "CL_INVALID_CONTEXT" // -34 + , "CL_INVALID_QUEUE_PROPERTIES" // -35 + , "CL_INVALID_COMMAND_QUEUE" // -36 + , "CL_INVALID_HOST_PTR" // -37 + , "CL_INVALID_MEM_OBJECT" // -38 + , "CL_INVALID_IMAGE_FORMAT_DESCRIPTOR" // -39 + , "CL_INVALID_IMAGE_SIZE" // -40 + , "CL_INVALID_SAMPLER" // -41 + , "CL_INVALID_BINARY" // -42 + , "CL_INVALID_BUILD_OPTIONS" // -43 + , "CL_INVALID_PROGRAM" // -44 + , "CL_INVALID_PROGRAM_EXECUTABLE" // -45 + , "CL_INVALID_KERNEL_NAME" // -46 + , "CL_INVALID_KERNEL_DEFINITION" // -47 + , "CL_INVALID_KERNEL" // -48 + , "CL_INVALID_ARG_INDEX" // -49 + , "CL_INVALID_ARG_VALUE" // -50 + , "CL_INVALID_ARG_SIZE" // -51 + , "CL_INVALID_KERNEL_ARGS" // -52 + , "CL_INVALID_WORK_DIMENSION" // -53 + , "CL_INVALID_WORK_GROUP_SIZE" // -54 + , "CL_INVALID_WORK_ITEM_SIZE" // -55 + , "CL_INVALID_GLOBAL_OFFSET" // -56 + , "CL_INVALID_EVENT_WAIT_LIST" // -57 + , "CL_INVALID_EVENT" // -58 + , "CL_INVALID_OPERATION" // -59 + , "CL_INVALID_GL_OBJECT" // -60 + , "CL_INVALID_BUFFER_SIZE" // -61 + , "CL_INVALID_MIP_LEVEL" // -62 + , "CL_INVALID_GLOBAL_WORK_SIZE" // -63 + }; + + return strings[-error]; +} diff --git a/extern/bullet/src/clew/clew.h b/extern/bullet/src/clew/clew.h new file mode 100644 index 000000000000..ee0fef18b414 --- /dev/null +++ b/extern/bullet/src/clew/clew.h @@ -0,0 +1,2397 @@ +#ifndef CLEW_HPP_INCLUDED +#define CLEW_HPP_INCLUDED + +////////////////////////////////////////////////////////////////////////// +// Copyright (c) 2009-2011 Organic Vectory B.V., KindDragon +// Written by George van Venrooij +// +// Distributed under the MIT License. +////////////////////////////////////////////////////////////////////////// + +//! \file clew.h +//! \brief OpenCL run-time loader header +//! +//! This file contains a copy of the contents of CL.H and CL_PLATFORM.H from the +//! official OpenCL spec. The purpose of this code is to load the OpenCL dynamic +//! library at run-time and thus allow the executable to function on many +//! platforms regardless of the vendor of the OpenCL driver actually installed. +//! Some of the techniques used here were inspired by work done in the GLEW +//! library (http://glew.sourceforge.net/) + +// Run-time dynamic linking functionality based on concepts used in GLEW +#ifdef __OPENCL_CL_H +#error cl.h included before clew.h +#endif + +#ifdef __OPENCL_CL_PLATFORM_H +#error cl_platform.h included before clew.h +#endif + +// Prevent cl.h inclusion +#define __OPENCL_CL_H +// Prevent cl_platform.h inclusion +#define __CL_PLATFORM_H + +/******************************************************************************* +* Copyright (c) 2008-2010 The Khronos Group Inc. +* +* Permission is hereby granted, free of charge, to any person obtaining a +* copy of this software and/or associated documentation files (the +* "Materials"), to deal in the Materials without restriction, including +* without limitation the rights to use, copy, modify, merge, publish, +* distribute, sublicense, and/or sell copies of the Materials, and to +* permit persons to whom the Materials are furnished to do so, subject to +* the following conditions: +* +* The above copyright notice and this permission notice shall be included +* in all copies or substantial portions of the Materials. +* +* THE MATERIALS ARE PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY +* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, +* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +* MATERIALS OR THE USE OR OTHER DEALINGS IN THE MATERIALS. +******************************************************************************/ +#ifdef __APPLE__ + /* Contains #defines for AVAILABLE_MAC_OS_X_VERSION_10_6_AND_LATER below */ + #include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(_WIN32) + #define CL_API_ENTRY + #define CL_API_CALL __stdcall + #define CL_CALLBACK __stdcall +#else + #define CL_API_ENTRY + #define CL_API_CALL + #define CL_CALLBACK +#endif +//disabled the APPLE thing, don't know why it is there, is just causes tons of warnings + +#ifdef __APPLE1__ + #define CL_EXTENSION_WEAK_LINK __attribute__((weak_import)) + #define CL_API_SUFFIX__VERSION_1_0 AVAILABLE_MAC_OS_X_VERSION_10_6_AND_LATER + #define CL_EXT_SUFFIX__VERSION_1_0 CL_EXTENSION_WEAK_LINK AVAILABLE_MAC_OS_X_VERSION_10_6_AND_LATER + #define CL_API_SUFFIX__VERSION_1_1 CL_EXTENSION_WEAK_LINK + #define CL_EXT_SUFFIX__VERSION_1_1 CL_EXTENSION_WEAK_LINK + #define CL_EXT_SUFFIX__VERSION_1_0_DEPRECATED CL_EXTENSION_WEAK_LINK AVAILABLE_MAC_OS_X_VERSION_10_6_AND_LATER +#else + #define CL_EXTENSION_WEAK_LINK + #define CL_API_SUFFIX__VERSION_1_0 + #define CL_EXT_SUFFIX__VERSION_1_0 + #define CL_API_SUFFIX__VERSION_1_1 + #define CL_EXT_SUFFIX__VERSION_1_1 + #define CL_EXT_SUFFIX__VERSION_1_0_DEPRECATED +#endif + +#if (defined (_WIN32) && defined(_MSC_VER)) + +/* scalar types */ +typedef signed __int8 cl_char; +typedef unsigned __int8 cl_uchar; +typedef signed __int16 cl_short; +typedef unsigned __int16 cl_ushort; +typedef signed __int32 cl_int; +typedef unsigned __int32 cl_uint; +typedef signed __int64 cl_long; +typedef unsigned __int64 cl_ulong; + +typedef unsigned __int16 cl_half; +typedef float cl_float; +typedef double cl_double; + +/* Macro names and corresponding values defined by OpenCL */ +#define CL_CHAR_BIT 8 +#define CL_SCHAR_MAX 127 +#define CL_SCHAR_MIN (-127-1) +#define CL_CHAR_MAX CL_SCHAR_MAX +#define CL_CHAR_MIN CL_SCHAR_MIN +#define CL_UCHAR_MAX 255 +#define CL_SHRT_MAX 32767 +#define CL_SHRT_MIN (-32767-1) +#define CL_USHRT_MAX 65535 +#define CL_INT_MAX 2147483647 +#define CL_INT_MIN (-2147483647-1) +#define CL_UINT_MAX 0xffffffffU +#define CL_LONG_MAX ((cl_long) 0x7FFFFFFFFFFFFFFFLL) +#define CL_LONG_MIN ((cl_long) -0x7FFFFFFFFFFFFFFFLL - 1LL) +#define CL_ULONG_MAX ((cl_ulong) 0xFFFFFFFFFFFFFFFFULL) + +#define CL_FLT_DIG 6 +#define CL_FLT_MANT_DIG 24 +#define CL_FLT_MAX_10_EXP +38 +#define CL_FLT_MAX_EXP +128 +#define CL_FLT_MIN_10_EXP -37 +#define CL_FLT_MIN_EXP -125 +#define CL_FLT_RADIX 2 +#define CL_FLT_MAX 340282346638528859811704183484516925440.0f +#define CL_FLT_MIN 1.175494350822287507969e-38f +#define CL_FLT_EPSILON 0x1.0p-23f + +#define CL_DBL_DIG 15 +#define CL_DBL_MANT_DIG 53 +#define CL_DBL_MAX_10_EXP +308 +#define CL_DBL_MAX_EXP +1024 +#define CL_DBL_MIN_10_EXP -307 +#define CL_DBL_MIN_EXP -1021 +#define CL_DBL_RADIX 2 +#define CL_DBL_MAX 179769313486231570814527423731704356798070567525844996598917476803157260780028538760589558632766878171540458953514382464234321326889464182768467546703537516986049910576551282076245490090389328944075868508455133942304583236903222948165808559332123348274797826204144723168738177180919299881250404026184124858368.0 +#define CL_DBL_MIN 2.225073858507201383090e-308 +#define CL_DBL_EPSILON 2.220446049250313080847e-16 + +#define CL_M_E 2.718281828459045090796 +#define CL_M_LOG2E 1.442695040888963387005 +#define CL_M_LOG10E 0.434294481903251816668 +#define CL_M_LN2 0.693147180559945286227 +#define CL_M_LN10 2.302585092994045901094 +#define CL_M_PI 3.141592653589793115998 +#define CL_M_PI_2 1.570796326794896557999 +#define CL_M_PI_4 0.785398163397448278999 +#define CL_M_1_PI 0.318309886183790691216 +#define CL_M_2_PI 0.636619772367581382433 +#define CL_M_2_SQRTPI 1.128379167095512558561 +#define CL_M_SQRT2 1.414213562373095145475 +#define CL_M_SQRT1_2 0.707106781186547572737 + +#define CL_M_E_F 2.71828174591064f +#define CL_M_LOG2E_F 1.44269502162933f +#define CL_M_LOG10E_F 0.43429449200630f +#define CL_M_LN2_F 0.69314718246460f +#define CL_M_LN10_F 2.30258512496948f +#define CL_M_PI_F 3.14159274101257f +#define CL_M_PI_2_F 1.57079637050629f +#define CL_M_PI_4_F 0.78539818525314f +#define CL_M_1_PI_F 0.31830987334251f +#define CL_M_2_PI_F 0.63661974668503f +#define CL_M_2_SQRTPI_F 1.12837922573090f +#define CL_M_SQRT2_F 1.41421353816986f +#define CL_M_SQRT1_2_F 0.70710676908493f + +#define CL_NAN (CL_INFINITY - CL_INFINITY) +#define CL_HUGE_VALF ((cl_float) 1e50) +#define CL_HUGE_VAL ((cl_double) 1e500) +#define CL_MAXFLOAT CL_FLT_MAX +#define CL_INFINITY CL_HUGE_VALF + +#else + +#include + +/* scalar types */ +typedef int8_t cl_char; +typedef uint8_t cl_uchar; +typedef int16_t cl_short __attribute__((aligned(2))); +typedef uint16_t cl_ushort __attribute__((aligned(2))); +typedef int32_t cl_int __attribute__((aligned(4))); +typedef uint32_t cl_uint __attribute__((aligned(4))); +typedef int64_t cl_long __attribute__((aligned(8))); +typedef uint64_t cl_ulong __attribute__((aligned(8))); + +typedef uint16_t cl_half __attribute__((aligned(2))); +typedef float cl_float __attribute__((aligned(4))); +typedef double cl_double __attribute__((aligned(8))); + +/* Macro names and corresponding values defined by OpenCL */ +#define CL_CHAR_BIT 8 +#define CL_SCHAR_MAX 127 +#define CL_SCHAR_MIN (-127-1) +#define CL_CHAR_MAX CL_SCHAR_MAX +#define CL_CHAR_MIN CL_SCHAR_MIN +#define CL_UCHAR_MAX 255 +#define CL_SHRT_MAX 32767 +#define CL_SHRT_MIN (-32767-1) +#define CL_USHRT_MAX 65535 +#define CL_INT_MAX 2147483647 +#define CL_INT_MIN (-2147483647-1) +#define CL_UINT_MAX 0xffffffffU +#define CL_LONG_MAX ((cl_long) 0x7FFFFFFFFFFFFFFFLL) +#define CL_LONG_MIN ((cl_long) -0x7FFFFFFFFFFFFFFFLL - 1LL) +#define CL_ULONG_MAX ((cl_ulong) 0xFFFFFFFFFFFFFFFFULL) + +#define CL_FLT_DIG 6 +#define CL_FLT_MANT_DIG 24 +#define CL_FLT_MAX_10_EXP +38 +#define CL_FLT_MAX_EXP +128 +#define CL_FLT_MIN_10_EXP -37 +#define CL_FLT_MIN_EXP -125 +#define CL_FLT_RADIX 2 +#define CL_FLT_MAX 0x1.fffffep127f +#define CL_FLT_MIN 0x1.0p-126f +#define CL_FLT_EPSILON 0x1.0p-23f + +#define CL_DBL_DIG 15 +#define CL_DBL_MANT_DIG 53 +#define CL_DBL_MAX_10_EXP +308 +#define CL_DBL_MAX_EXP +1024 +#define CL_DBL_MIN_10_EXP -307 +#define CL_DBL_MIN_EXP -1021 +#define CL_DBL_RADIX 2 +#define CL_DBL_MAX 0x1.fffffffffffffp1023 +#define CL_DBL_MIN 0x1.0p-1022 +#define CL_DBL_EPSILON 0x1.0p-52 + +#define CL_M_E 2.718281828459045090796 +#define CL_M_LOG2E 1.442695040888963387005 +#define CL_M_LOG10E 0.434294481903251816668 +#define CL_M_LN2 0.693147180559945286227 +#define CL_M_LN10 2.302585092994045901094 +#define CL_M_PI 3.141592653589793115998 +#define CL_M_PI_2 1.570796326794896557999 +#define CL_M_PI_4 0.785398163397448278999 +#define CL_M_1_PI 0.318309886183790691216 +#define CL_M_2_PI 0.636619772367581382433 +#define CL_M_2_SQRTPI 1.128379167095512558561 +#define CL_M_SQRT2 1.414213562373095145475 +#define CL_M_SQRT1_2 0.707106781186547572737 + +#define CL_M_E_F 2.71828174591064f +#define CL_M_LOG2E_F 1.44269502162933f +#define CL_M_LOG10E_F 0.43429449200630f +#define CL_M_LN2_F 0.69314718246460f +#define CL_M_LN10_F 2.30258512496948f +#define CL_M_PI_F 3.14159274101257f +#define CL_M_PI_2_F 1.57079637050629f +#define CL_M_PI_4_F 0.78539818525314f +#define CL_M_1_PI_F 0.31830987334251f +#define CL_M_2_PI_F 0.63661974668503f +#define CL_M_2_SQRTPI_F 1.12837922573090f +#define CL_M_SQRT2_F 1.41421353816986f +#define CL_M_SQRT1_2_F 0.70710676908493f + +#if defined( __GNUC__ ) + #define CL_HUGE_VALF __builtin_huge_valf() + #define CL_HUGE_VAL __builtin_huge_val() + #define CL_NAN __builtin_nanf( "" ) +#else + #define CL_HUGE_VALF ((cl_float) 1e50) + #define CL_HUGE_VAL ((cl_double) 1e500) + float nanf( const char * ); + #define CL_NAN nanf( "" ) +#endif +#define CL_MAXFLOAT CL_FLT_MAX +#define CL_INFINITY CL_HUGE_VALF + +#endif + +#include + +/* Mirror types to GL types. Mirror types allow us to avoid deciding which headers to load based on whether we are using GL or GLES here. */ +typedef unsigned int cl_GLuint; +typedef int cl_GLint; +typedef unsigned int cl_GLenum; + +/* + * Vector types + * + * Note: OpenCL requires that all types be naturally aligned. + * This means that vector types must be naturally aligned. + * For example, a vector of four floats must be aligned to + * a 16 byte boundary (calculated as 4 * the natural 4-byte + * alignment of the float). The alignment qualifiers here + * will only function properly if your compiler supports them + * and if you don't actively work to defeat them. For example, + * in order for a cl_float4 to be 16 byte aligned in a struct, + * the start of the struct must itself be 16-byte aligned. + * + * Maintaining proper alignment is the user's responsibility. + */ + + +#ifdef _MSC_VER +#if defined(_M_IX86) +#if _M_IX86_FP >= 0 +#define __SSE__ +#endif +#if _M_IX86_FP >= 1 +#define __SSE2__ +#endif +#elif defined(_M_X64) +#define __SSE__ +#define __SSE2__ +#endif +#endif + +/* Define basic vector types */ +#if defined( __VEC__ ) + #include /* may be omitted depending on compiler. AltiVec spec provides no way to detect whether the header is required. */ + typedef vector unsigned char __cl_uchar16; + typedef vector signed char __cl_char16; + typedef vector unsigned short __cl_ushort8; + typedef vector signed short __cl_short8; + typedef vector unsigned int __cl_uint4; + typedef vector signed int __cl_int4; + typedef vector float __cl_float4; + #define __CL_UCHAR16__ 1 + #define __CL_CHAR16__ 1 + #define __CL_USHORT8__ 1 + #define __CL_SHORT8__ 1 + #define __CL_UINT4__ 1 + #define __CL_INT4__ 1 + #define __CL_FLOAT4__ 1 +#endif + +#if defined( __SSE__ ) + #if defined( __MINGW64__ ) + #include + #else + #include + #endif + #if defined( __GNUC__ ) && !defined( __ICC ) + typedef float __cl_float4 __attribute__((vector_size(16))); + #else + typedef __m128 __cl_float4; + #endif + #define __CL_FLOAT4__ 1 +#endif + +#if defined( __SSE2__ ) + #if defined( __MINGW64__ ) + #include + #else + #include + #endif + #if defined( __GNUC__ ) && !defined( __ICC ) + typedef cl_uchar __cl_uchar16 __attribute__((vector_size(16))); + typedef cl_char __cl_char16 __attribute__((vector_size(16))); + typedef cl_ushort __cl_ushort8 __attribute__((vector_size(16))); + typedef cl_short __cl_short8 __attribute__((vector_size(16))); + typedef cl_uint __cl_uint4 __attribute__((vector_size(16))); + typedef cl_int __cl_int4 __attribute__((vector_size(16))); + typedef cl_ulong __cl_ulong2 __attribute__((vector_size(16))); + typedef cl_long __cl_long2 __attribute__((vector_size(16))); + typedef cl_double __cl_double2 __attribute__((vector_size(16))); + #else + typedef __m128i __cl_uchar16; + typedef __m128i __cl_char16; + typedef __m128i __cl_ushort8; + typedef __m128i __cl_short8; + typedef __m128i __cl_uint4; + typedef __m128i __cl_int4; + typedef __m128i __cl_ulong2; + typedef __m128i __cl_long2; + typedef __m128d __cl_double2; + #endif + #define __CL_UCHAR16__ 1 + #define __CL_CHAR16__ 1 + #define __CL_USHORT8__ 1 + #define __CL_SHORT8__ 1 + #define __CL_INT4__ 1 + #define __CL_UINT4__ 1 + #define __CL_ULONG2__ 1 + #define __CL_LONG2__ 1 + #define __CL_DOUBLE2__ 1 +#endif + +#if defined( __MMX__ ) + #include + #if defined( __GNUC__ ) && !defined( __ICC ) + typedef cl_uchar __cl_uchar8 __attribute__((vector_size(8))); + typedef cl_char __cl_char8 __attribute__((vector_size(8))); + typedef cl_ushort __cl_ushort4 __attribute__((vector_size(8))); + typedef cl_short __cl_short4 __attribute__((vector_size(8))); + typedef cl_uint __cl_uint2 __attribute__((vector_size(8))); + typedef cl_int __cl_int2 __attribute__((vector_size(8))); + typedef cl_ulong __cl_ulong1 __attribute__((vector_size(8))); + typedef cl_long __cl_long1 __attribute__((vector_size(8))); + typedef cl_float __cl_float2 __attribute__((vector_size(8))); + #else + typedef __m64 __cl_uchar8; + typedef __m64 __cl_char8; + typedef __m64 __cl_ushort4; + typedef __m64 __cl_short4; + typedef __m64 __cl_uint2; + typedef __m64 __cl_int2; + typedef __m64 __cl_ulong1; + typedef __m64 __cl_long1; + typedef __m64 __cl_float2; + #endif + #define __CL_UCHAR8__ 1 + #define __CL_CHAR8__ 1 + #define __CL_USHORT4__ 1 + #define __CL_SHORT4__ 1 + #define __CL_INT2__ 1 + #define __CL_UINT2__ 1 + #define __CL_ULONG1__ 1 + #define __CL_LONG1__ 1 + #define __CL_FLOAT2__ 1 +#endif + +#if defined( __AVX__ ) + #if defined( __MINGW64__ ) + #include + #else + #include + #endif + #if defined( __GNUC__ ) && !defined( __ICC ) + typedef cl_float __cl_float8 __attribute__((vector_size(32))); + typedef cl_double __cl_double4 __attribute__((vector_size(32))); + #else + typedef __m256 __cl_float8; + typedef __m256d __cl_double4; + #endif + #define __CL_FLOAT8__ 1 + #define __CL_DOUBLE4__ 1 +#endif + +/* Define alignment keys */ +#if defined( __GNUC__ ) + #define CL_ALIGNED(_x) __attribute__ ((aligned(_x))) +#elif defined( _WIN32) && (_MSC_VER) + /* Alignment keys neutered on windows because MSVC can't swallow function arguments with alignment requirements */ + /* http://msdn.microsoft.com/en-us/library/373ak2y1%28VS.71%29.aspx */ + /* #include */ + /* #define CL_ALIGNED(_x) _CRT_ALIGN(_x) */ + #define CL_ALIGNED(_x) +#else + #warning Need to implement some method to align data here + #define CL_ALIGNED(_x) +#endif + +/* Indicate whether .xyzw, .s0123 and .hi.lo are supported */ +#if (defined( __GNUC__) && ! defined( __STRICT_ANSI__ )) || (defined( _MSC_VER ) && ! defined( __STDC__ )) + /* .xyzw and .s0123...{f|F} are supported */ + #define CL_HAS_NAMED_VECTOR_FIELDS 1 + /* .hi and .lo are supported */ + #define CL_HAS_HI_LO_VECTOR_FIELDS 1 + + #define CL_NAMED_STRUCT_SUPPORTED +#endif + +#if defined( CL_NAMED_STRUCT_SUPPORTED) && defined( _MSC_VER ) +#define __extension__ __pragma(warning(suppress:4201)) +#endif + +/* Define cl_vector types */ + +/* ---- cl_charn ---- */ +typedef union +{ + cl_char CL_ALIGNED(2) s[2]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_char x, y; }; + __extension__ struct{ cl_char s0, s1; }; + __extension__ struct{ cl_char lo, hi; }; +#endif +#if defined( __CL_CHAR2__) + __cl_char2 v2; +#endif +}cl_char2; + +typedef union +{ + cl_char CL_ALIGNED(4) s[4]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_char x, y, z, w; }; + __extension__ struct{ cl_char s0, s1, s2, s3; }; + __extension__ struct{ cl_char2 lo, hi; }; +#endif +#if defined( __CL_CHAR2__) + __cl_char2 v2[2]; +#endif +#if defined( __CL_CHAR4__) + __cl_char4 v4; +#endif +}cl_char4; + +/* cl_char3 is identical in size, alignment and behavior to cl_char4. See section 6.1.5. */ +typedef cl_char4 cl_char3; + +typedef union +{ + cl_char CL_ALIGNED(8) s[8]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_char x, y, z, w; }; + __extension__ struct{ cl_char s0, s1, s2, s3, s4, s5, s6, s7; }; + __extension__ struct{ cl_char4 lo, hi; }; +#endif +#if defined( __CL_CHAR2__) + __cl_char2 v2[4]; +#endif +#if defined( __CL_CHAR4__) + __cl_char4 v4[2]; +#endif +#if defined( __CL_CHAR8__ ) + __cl_char8 v8; +#endif +}cl_char8; + +typedef union +{ + cl_char CL_ALIGNED(16) s[16]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_char x, y, z, w, __spacer4, __spacer5, __spacer6, __spacer7, __spacer8, __spacer9, sa, sb, sc, sd, se, sf; }; + __extension__ struct{ cl_char s0, s1, s2, s3, s4, s5, s6, s7, s8, s9, sA, sB, sC, sD, sE, sF; }; + __extension__ struct{ cl_char8 lo, hi; }; +#endif +#if defined( __CL_CHAR2__) + __cl_char2 v2[8]; +#endif +#if defined( __CL_CHAR4__) + __cl_char4 v4[4]; +#endif +#if defined( __CL_CHAR8__ ) + __cl_char8 v8[2]; +#endif +#if defined( __CL_CHAR16__ ) + __cl_char16 v16; +#endif +}cl_char16; + + +/* ---- cl_ucharn ---- */ +typedef union +{ + cl_uchar CL_ALIGNED(2) s[2]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_uchar x, y; }; + __extension__ struct{ cl_uchar s0, s1; }; + __extension__ struct{ cl_uchar lo, hi; }; +#endif +#if defined( __cl_uchar2__) + __cl_uchar2 v2; +#endif +}cl_uchar2; + +typedef union +{ + cl_uchar CL_ALIGNED(4) s[4]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_uchar x, y, z, w; }; + __extension__ struct{ cl_uchar s0, s1, s2, s3; }; + __extension__ struct{ cl_uchar2 lo, hi; }; +#endif +#if defined( __CL_UCHAR2__) + __cl_uchar2 v2[2]; +#endif +#if defined( __CL_UCHAR4__) + __cl_uchar4 v4; +#endif +}cl_uchar4; + +/* cl_uchar3 is identical in size, alignment and behavior to cl_uchar4. See section 6.1.5. */ +typedef cl_uchar4 cl_uchar3; + +typedef union +{ + cl_uchar CL_ALIGNED(8) s[8]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_uchar x, y, z, w; }; + __extension__ struct{ cl_uchar s0, s1, s2, s3, s4, s5, s6, s7; }; + __extension__ struct{ cl_uchar4 lo, hi; }; +#endif +#if defined( __CL_UCHAR2__) + __cl_uchar2 v2[4]; +#endif +#if defined( __CL_UCHAR4__) + __cl_uchar4 v4[2]; +#endif +#if defined( __CL_UCHAR8__ ) + __cl_uchar8 v8; +#endif +}cl_uchar8; + +typedef union +{ + cl_uchar CL_ALIGNED(16) s[16]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_uchar x, y, z, w, __spacer4, __spacer5, __spacer6, __spacer7, __spacer8, __spacer9, sa, sb, sc, sd, se, sf; }; + __extension__ struct{ cl_uchar s0, s1, s2, s3, s4, s5, s6, s7, s8, s9, sA, sB, sC, sD, sE, sF; }; + __extension__ struct{ cl_uchar8 lo, hi; }; +#endif +#if defined( __CL_UCHAR2__) + __cl_uchar2 v2[8]; +#endif +#if defined( __CL_UCHAR4__) + __cl_uchar4 v4[4]; +#endif +#if defined( __CL_UCHAR8__ ) + __cl_uchar8 v8[2]; +#endif +#if defined( __CL_UCHAR16__ ) + __cl_uchar16 v16; +#endif +}cl_uchar16; + + +/* ---- cl_shortn ---- */ +typedef union +{ + cl_short CL_ALIGNED(4) s[2]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_short x, y; }; + __extension__ struct{ cl_short s0, s1; }; + __extension__ struct{ cl_short lo, hi; }; +#endif +#if defined( __CL_SHORT2__) + __cl_short2 v2; +#endif +}cl_short2; + +typedef union +{ + cl_short CL_ALIGNED(8) s[4]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_short x, y, z, w; }; + __extension__ struct{ cl_short s0, s1, s2, s3; }; + __extension__ struct{ cl_short2 lo, hi; }; +#endif +#if defined( __CL_SHORT2__) + __cl_short2 v2[2]; +#endif +#if defined( __CL_SHORT4__) + __cl_short4 v4; +#endif +}cl_short4; + +/* cl_short3 is identical in size, alignment and behavior to cl_short4. See section 6.1.5. */ +typedef cl_short4 cl_short3; + +typedef union +{ + cl_short CL_ALIGNED(16) s[8]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_short x, y, z, w; }; + __extension__ struct{ cl_short s0, s1, s2, s3, s4, s5, s6, s7; }; + __extension__ struct{ cl_short4 lo, hi; }; +#endif +#if defined( __CL_SHORT2__) + __cl_short2 v2[4]; +#endif +#if defined( __CL_SHORT4__) + __cl_short4 v4[2]; +#endif +#if defined( __CL_SHORT8__ ) + __cl_short8 v8; +#endif +}cl_short8; + +typedef union +{ + cl_short CL_ALIGNED(32) s[16]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_short x, y, z, w, __spacer4, __spacer5, __spacer6, __spacer7, __spacer8, __spacer9, sa, sb, sc, sd, se, sf; }; + __extension__ struct{ cl_short s0, s1, s2, s3, s4, s5, s6, s7, s8, s9, sA, sB, sC, sD, sE, sF; }; + __extension__ struct{ cl_short8 lo, hi; }; +#endif +#if defined( __CL_SHORT2__) + __cl_short2 v2[8]; +#endif +#if defined( __CL_SHORT4__) + __cl_short4 v4[4]; +#endif +#if defined( __CL_SHORT8__ ) + __cl_short8 v8[2]; +#endif +#if defined( __CL_SHORT16__ ) + __cl_short16 v16; +#endif +}cl_short16; + + +/* ---- cl_ushortn ---- */ +typedef union +{ + cl_ushort CL_ALIGNED(4) s[2]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_ushort x, y; }; + __extension__ struct{ cl_ushort s0, s1; }; + __extension__ struct{ cl_ushort lo, hi; }; +#endif +#if defined( __CL_USHORT2__) + __cl_ushort2 v2; +#endif +}cl_ushort2; + +typedef union +{ + cl_ushort CL_ALIGNED(8) s[4]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_ushort x, y, z, w; }; + __extension__ struct{ cl_ushort s0, s1, s2, s3; }; + __extension__ struct{ cl_ushort2 lo, hi; }; +#endif +#if defined( __CL_USHORT2__) + __cl_ushort2 v2[2]; +#endif +#if defined( __CL_USHORT4__) + __cl_ushort4 v4; +#endif +}cl_ushort4; + +/* cl_ushort3 is identical in size, alignment and behavior to cl_ushort4. See section 6.1.5. */ +typedef cl_ushort4 cl_ushort3; + +typedef union +{ + cl_ushort CL_ALIGNED(16) s[8]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_ushort x, y, z, w; }; + __extension__ struct{ cl_ushort s0, s1, s2, s3, s4, s5, s6, s7; }; + __extension__ struct{ cl_ushort4 lo, hi; }; +#endif +#if defined( __CL_USHORT2__) + __cl_ushort2 v2[4]; +#endif +#if defined( __CL_USHORT4__) + __cl_ushort4 v4[2]; +#endif +#if defined( __CL_USHORT8__ ) + __cl_ushort8 v8; +#endif +}cl_ushort8; + +typedef union +{ + cl_ushort CL_ALIGNED(32) s[16]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_ushort x, y, z, w, __spacer4, __spacer5, __spacer6, __spacer7, __spacer8, __spacer9, sa, sb, sc, sd, se, sf; }; + __extension__ struct{ cl_ushort s0, s1, s2, s3, s4, s5, s6, s7, s8, s9, sA, sB, sC, sD, sE, sF; }; + __extension__ struct{ cl_ushort8 lo, hi; }; +#endif +#if defined( __CL_USHORT2__) + __cl_ushort2 v2[8]; +#endif +#if defined( __CL_USHORT4__) + __cl_ushort4 v4[4]; +#endif +#if defined( __CL_USHORT8__ ) + __cl_ushort8 v8[2]; +#endif +#if defined( __CL_USHORT16__ ) + __cl_ushort16 v16; +#endif +}cl_ushort16; + +/* ---- cl_intn ---- */ +typedef union +{ + cl_int CL_ALIGNED(8) s[2]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_int x, y; }; + __extension__ struct{ cl_int s0, s1; }; + __extension__ struct{ cl_int lo, hi; }; +#endif +#if defined( __CL_INT2__) + __cl_int2 v2; +#endif +}cl_int2; + +typedef union +{ + cl_int CL_ALIGNED(16) s[4]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_int x, y, z, w; }; + __extension__ struct{ cl_int s0, s1, s2, s3; }; + __extension__ struct{ cl_int2 lo, hi; }; +#endif +#if defined( __CL_INT2__) + __cl_int2 v2[2]; +#endif +#if defined( __CL_INT4__) + __cl_int4 v4; +#endif +}cl_int4; + +/* cl_int3 is identical in size, alignment and behavior to cl_int4. See section 6.1.5. */ +typedef cl_int4 cl_int3; + +typedef union +{ + cl_int CL_ALIGNED(32) s[8]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_int x, y, z, w; }; + __extension__ struct{ cl_int s0, s1, s2, s3, s4, s5, s6, s7; }; + __extension__ struct{ cl_int4 lo, hi; }; +#endif +#if defined( __CL_INT2__) + __cl_int2 v2[4]; +#endif +#if defined( __CL_INT4__) + __cl_int4 v4[2]; +#endif +#if defined( __CL_INT8__ ) + __cl_int8 v8; +#endif +}cl_int8; + +typedef union +{ + cl_int CL_ALIGNED(64) s[16]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_int x, y, z, w, __spacer4, __spacer5, __spacer6, __spacer7, __spacer8, __spacer9, sa, sb, sc, sd, se, sf; }; + __extension__ struct{ cl_int s0, s1, s2, s3, s4, s5, s6, s7, s8, s9, sA, sB, sC, sD, sE, sF; }; + __extension__ struct{ cl_int8 lo, hi; }; +#endif +#if defined( __CL_INT2__) + __cl_int2 v2[8]; +#endif +#if defined( __CL_INT4__) + __cl_int4 v4[4]; +#endif +#if defined( __CL_INT8__ ) + __cl_int8 v8[2]; +#endif +#if defined( __CL_INT16__ ) + __cl_int16 v16; +#endif +}cl_int16; + + +/* ---- cl_uintn ---- */ +typedef union +{ + cl_uint CL_ALIGNED(8) s[2]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_uint x, y; }; + __extension__ struct{ cl_uint s0, s1; }; + __extension__ struct{ cl_uint lo, hi; }; +#endif +#if defined( __CL_UINT2__) + __cl_uint2 v2; +#endif +}cl_uint2; + +typedef union +{ + cl_uint CL_ALIGNED(16) s[4]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_uint x, y, z, w; }; + __extension__ struct{ cl_uint s0, s1, s2, s3; }; + __extension__ struct{ cl_uint2 lo, hi; }; +#endif +#if defined( __CL_UINT2__) + __cl_uint2 v2[2]; +#endif +#if defined( __CL_UINT4__) + __cl_uint4 v4; +#endif +}cl_uint4; + +/* cl_uint3 is identical in size, alignment and behavior to cl_uint4. See section 6.1.5. */ +typedef cl_uint4 cl_uint3; + +typedef union +{ + cl_uint CL_ALIGNED(32) s[8]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_uint x, y, z, w; }; + __extension__ struct{ cl_uint s0, s1, s2, s3, s4, s5, s6, s7; }; + __extension__ struct{ cl_uint4 lo, hi; }; +#endif +#if defined( __CL_UINT2__) + __cl_uint2 v2[4]; +#endif +#if defined( __CL_UINT4__) + __cl_uint4 v4[2]; +#endif +#if defined( __CL_UINT8__ ) + __cl_uint8 v8; +#endif +}cl_uint8; + +typedef union +{ + cl_uint CL_ALIGNED(64) s[16]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_uint x, y, z, w, __spacer4, __spacer5, __spacer6, __spacer7, __spacer8, __spacer9, sa, sb, sc, sd, se, sf; }; + __extension__ struct{ cl_uint s0, s1, s2, s3, s4, s5, s6, s7, s8, s9, sA, sB, sC, sD, sE, sF; }; + __extension__ struct{ cl_uint8 lo, hi; }; +#endif +#if defined( __CL_UINT2__) + __cl_uint2 v2[8]; +#endif +#if defined( __CL_UINT4__) + __cl_uint4 v4[4]; +#endif +#if defined( __CL_UINT8__ ) + __cl_uint8 v8[2]; +#endif +#if defined( __CL_UINT16__ ) + __cl_uint16 v16; +#endif +}cl_uint16; + +/* ---- cl_longn ---- */ +typedef union +{ + cl_long CL_ALIGNED(16) s[2]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_long x, y; }; + __extension__ struct{ cl_long s0, s1; }; + __extension__ struct{ cl_long lo, hi; }; +#endif +#if defined( __CL_LONG2__) + __cl_long2 v2; +#endif +}cl_long2; + +typedef union +{ + cl_long CL_ALIGNED(32) s[4]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_long x, y, z, w; }; + __extension__ struct{ cl_long s0, s1, s2, s3; }; + __extension__ struct{ cl_long2 lo, hi; }; +#endif +#if defined( __CL_LONG2__) + __cl_long2 v2[2]; +#endif +#if defined( __CL_LONG4__) + __cl_long4 v4; +#endif +}cl_long4; + +/* cl_long3 is identical in size, alignment and behavior to cl_long4. See section 6.1.5. */ +typedef cl_long4 cl_long3; + +typedef union +{ + cl_long CL_ALIGNED(64) s[8]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_long x, y, z, w; }; + __extension__ struct{ cl_long s0, s1, s2, s3, s4, s5, s6, s7; }; + __extension__ struct{ cl_long4 lo, hi; }; +#endif +#if defined( __CL_LONG2__) + __cl_long2 v2[4]; +#endif +#if defined( __CL_LONG4__) + __cl_long4 v4[2]; +#endif +#if defined( __CL_LONG8__ ) + __cl_long8 v8; +#endif +}cl_long8; + +typedef union +{ + cl_long CL_ALIGNED(128) s[16]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_long x, y, z, w, __spacer4, __spacer5, __spacer6, __spacer7, __spacer8, __spacer9, sa, sb, sc, sd, se, sf; }; + __extension__ struct{ cl_long s0, s1, s2, s3, s4, s5, s6, s7, s8, s9, sA, sB, sC, sD, sE, sF; }; + __extension__ struct{ cl_long8 lo, hi; }; +#endif +#if defined( __CL_LONG2__) + __cl_long2 v2[8]; +#endif +#if defined( __CL_LONG4__) + __cl_long4 v4[4]; +#endif +#if defined( __CL_LONG8__ ) + __cl_long8 v8[2]; +#endif +#if defined( __CL_LONG16__ ) + __cl_long16 v16; +#endif +}cl_long16; + + +/* ---- cl_ulongn ---- */ +typedef union +{ + cl_ulong CL_ALIGNED(16) s[2]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_ulong x, y; }; + __extension__ struct{ cl_ulong s0, s1; }; + __extension__ struct{ cl_ulong lo, hi; }; +#endif +#if defined( __CL_ULONG2__) + __cl_ulong2 v2; +#endif +}cl_ulong2; + +typedef union +{ + cl_ulong CL_ALIGNED(32) s[4]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_ulong x, y, z, w; }; + __extension__ struct{ cl_ulong s0, s1, s2, s3; }; + __extension__ struct{ cl_ulong2 lo, hi; }; +#endif +#if defined( __CL_ULONG2__) + __cl_ulong2 v2[2]; +#endif +#if defined( __CL_ULONG4__) + __cl_ulong4 v4; +#endif +}cl_ulong4; + +/* cl_ulong3 is identical in size, alignment and behavior to cl_ulong4. See section 6.1.5. */ +typedef cl_ulong4 cl_ulong3; + +typedef union +{ + cl_ulong CL_ALIGNED(64) s[8]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_ulong x, y, z, w; }; + __extension__ struct{ cl_ulong s0, s1, s2, s3, s4, s5, s6, s7; }; + __extension__ struct{ cl_ulong4 lo, hi; }; +#endif +#if defined( __CL_ULONG2__) + __cl_ulong2 v2[4]; +#endif +#if defined( __CL_ULONG4__) + __cl_ulong4 v4[2]; +#endif +#if defined( __CL_ULONG8__ ) + __cl_ulong8 v8; +#endif +}cl_ulong8; + +typedef union +{ + cl_ulong CL_ALIGNED(128) s[16]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_ulong x, y, z, w, __spacer4, __spacer5, __spacer6, __spacer7, __spacer8, __spacer9, sa, sb, sc, sd, se, sf; }; + __extension__ struct{ cl_ulong s0, s1, s2, s3, s4, s5, s6, s7, s8, s9, sA, sB, sC, sD, sE, sF; }; + __extension__ struct{ cl_ulong8 lo, hi; }; +#endif +#if defined( __CL_ULONG2__) + __cl_ulong2 v2[8]; +#endif +#if defined( __CL_ULONG4__) + __cl_ulong4 v4[4]; +#endif +#if defined( __CL_ULONG8__ ) + __cl_ulong8 v8[2]; +#endif +#if defined( __CL_ULONG16__ ) + __cl_ulong16 v16; +#endif +}cl_ulong16; + + +/* --- cl_floatn ---- */ + +typedef union +{ + cl_float CL_ALIGNED(8) s[2]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_float x, y; }; + __extension__ struct{ cl_float s0, s1; }; + __extension__ struct{ cl_float lo, hi; }; +#endif +#if defined( __CL_FLOAT2__) + __cl_float2 v2; +#endif +}cl_float2; + +typedef union +{ + cl_float CL_ALIGNED(16) s[4]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_float x, y, z, w; }; + __extension__ struct{ cl_float s0, s1, s2, s3; }; + __extension__ struct{ cl_float2 lo, hi; }; +#endif +#if defined( __CL_FLOAT2__) + __cl_float2 v2[2]; +#endif +#if defined( __CL_FLOAT4__) + __cl_float4 v4; +#endif +}cl_float4; + +/* cl_float3 is identical in size, alignment and behavior to cl_float4. See section 6.1.5. */ +typedef cl_float4 cl_float3; + +typedef union +{ + cl_float CL_ALIGNED(32) s[8]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_float x, y, z, w; }; + __extension__ struct{ cl_float s0, s1, s2, s3, s4, s5, s6, s7; }; + __extension__ struct{ cl_float4 lo, hi; }; +#endif +#if defined( __CL_FLOAT2__) + __cl_float2 v2[4]; +#endif +#if defined( __CL_FLOAT4__) + __cl_float4 v4[2]; +#endif +#if defined( __CL_FLOAT8__ ) + __cl_float8 v8; +#endif +}cl_float8; + +typedef union +{ + cl_float CL_ALIGNED(64) s[16]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_float x, y, z, w, __spacer4, __spacer5, __spacer6, __spacer7, __spacer8, __spacer9, sa, sb, sc, sd, se, sf; }; + __extension__ struct{ cl_float s0, s1, s2, s3, s4, s5, s6, s7, s8, s9, sA, sB, sC, sD, sE, sF; }; + __extension__ struct{ cl_float8 lo, hi; }; +#endif +#if defined( __CL_FLOAT2__) + __cl_float2 v2[8]; +#endif +#if defined( __CL_FLOAT4__) + __cl_float4 v4[4]; +#endif +#if defined( __CL_FLOAT8__ ) + __cl_float8 v8[2]; +#endif +#if defined( __CL_FLOAT16__ ) + __cl_float16 v16; +#endif +}cl_float16; + +/* --- cl_doublen ---- */ + +typedef union +{ + cl_double CL_ALIGNED(16) s[2]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_double x, y; }; + __extension__ struct{ cl_double s0, s1; }; + __extension__ struct{ cl_double lo, hi; }; +#endif +#if defined( __CL_DOUBLE2__) + __cl_double2 v2; +#endif +}cl_double2; + +typedef union +{ + cl_double CL_ALIGNED(32) s[4]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_double x, y, z, w; }; + __extension__ struct{ cl_double s0, s1, s2, s3; }; + __extension__ struct{ cl_double2 lo, hi; }; +#endif +#if defined( __CL_DOUBLE2__) + __cl_double2 v2[2]; +#endif +#if defined( __CL_DOUBLE4__) + __cl_double4 v4; +#endif +}cl_double4; + +/* cl_double3 is identical in size, alignment and behavior to cl_double4. See section 6.1.5. */ +typedef cl_double4 cl_double3; + +typedef union +{ + cl_double CL_ALIGNED(64) s[8]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_double x, y, z, w; }; + __extension__ struct{ cl_double s0, s1, s2, s3, s4, s5, s6, s7; }; + __extension__ struct{ cl_double4 lo, hi; }; +#endif +#if defined( __CL_DOUBLE2__) + __cl_double2 v2[4]; +#endif +#if defined( __CL_DOUBLE4__) + __cl_double4 v4[2]; +#endif +#if defined( __CL_DOUBLE8__ ) + __cl_double8 v8; +#endif +}cl_double8; + +typedef union +{ + cl_double CL_ALIGNED(128) s[16]; +#if defined( CL_NAMED_STRUCT_SUPPORTED ) + __extension__ struct{ cl_double x, y, z, w, __spacer4, __spacer5, __spacer6, __spacer7, __spacer8, __spacer9, sa, sb, sc, sd, se, sf; }; + __extension__ struct{ cl_double s0, s1, s2, s3, s4, s5, s6, s7, s8, s9, sA, sB, sC, sD, sE, sF; }; + __extension__ struct{ cl_double8 lo, hi; }; +#endif +#if defined( __CL_DOUBLE2__) + __cl_double2 v2[8]; +#endif +#if defined( __CL_DOUBLE4__) + __cl_double4 v4[4]; +#endif +#if defined( __CL_DOUBLE8__ ) + __cl_double8 v8[2]; +#endif +#if defined( __CL_DOUBLE16__ ) + __cl_double16 v16; +#endif +}cl_double16; + +/* Macro to facilitate debugging + * Usage: + * Place CL_PROGRAM_STRING_DEBUG_INFO on the line before the first line of your source. + * The first line ends with: CL_PROGRAM_STRING_BEGIN \" + * Each line thereafter of OpenCL C source must end with: \n\ + * The last line ends in "; + * + * Example: + * + * const char *my_program = CL_PROGRAM_STRING_BEGIN "\ + * kernel void foo( int a, float * b ) \n\ + * { \n\ + * // my comment \n\ + * *b[ get_global_id(0)] = a; \n\ + * } \n\ + * "; + * + * This should correctly set up the line, (column) and file information for your source + * string so you can do source level debugging. + */ +#define __CL_STRINGIFY( _x ) # _x +#define _CL_STRINGIFY( _x ) __CL_STRINGIFY( _x ) +#define CL_PROGRAM_STRING_DEBUG_INFO "#line " _CL_STRINGIFY(__LINE__) " \"" __FILE__ "\" \n\n" + +// CL.h contents +/******************************************************************************/ + +typedef struct _cl_platform_id * cl_platform_id; +typedef struct _cl_device_id * cl_device_id; +typedef struct _cl_context * cl_context; +typedef struct _cl_command_queue * cl_command_queue; +typedef struct _cl_mem * cl_mem; +typedef struct _cl_program * cl_program; +typedef struct _cl_kernel * cl_kernel; +typedef struct _cl_event * cl_event; +typedef struct _cl_sampler * cl_sampler; + +typedef cl_uint cl_bool; /* WARNING! Unlike cl_ types in cl_platform.h, cl_bool is not guaranteed to be the same size as the bool in kernels. */ +typedef cl_ulong cl_bitfield; +typedef cl_bitfield cl_device_type; +typedef cl_uint cl_platform_info; +typedef cl_uint cl_device_info; +typedef cl_bitfield cl_device_fp_config; +typedef cl_uint cl_device_mem_cache_type; +typedef cl_uint cl_device_local_mem_type; +typedef cl_bitfield cl_device_exec_capabilities; +typedef cl_bitfield cl_command_queue_properties; + +typedef intptr_t cl_context_properties; +typedef cl_uint cl_context_info; +typedef cl_uint cl_command_queue_info; +typedef cl_uint cl_channel_order; +typedef cl_uint cl_channel_type; +typedef cl_bitfield cl_mem_flags; +typedef cl_uint cl_mem_object_type; +typedef cl_uint cl_mem_info; +typedef cl_uint cl_image_info; +typedef cl_uint cl_buffer_create_type; +typedef cl_uint cl_addressing_mode; +typedef cl_uint cl_filter_mode; +typedef cl_uint cl_sampler_info; +typedef cl_bitfield cl_map_flags; +typedef cl_uint cl_program_info; +typedef cl_uint cl_program_build_info; +typedef cl_int cl_build_status; +typedef cl_uint cl_kernel_info; +typedef cl_uint cl_kernel_work_group_info; +typedef cl_uint cl_event_info; +typedef cl_uint cl_command_type; +typedef cl_uint cl_profiling_info; + +typedef struct _cl_image_format { + cl_channel_order image_channel_order; + cl_channel_type image_channel_data_type; +} cl_image_format; + + +typedef struct _cl_buffer_region { + size_t origin; + size_t size; +} cl_buffer_region; + +/******************************************************************************/ + +/* Error Codes */ +#define CL_SUCCESS 0 +#define CL_DEVICE_NOT_FOUND -1 +#define CL_DEVICE_NOT_AVAILABLE -2 +#define CL_COMPILER_NOT_AVAILABLE -3 +#define CL_MEM_OBJECT_ALLOCATION_FAILURE -4 +#define CL_OUT_OF_RESOURCES -5 +#define CL_OUT_OF_HOST_MEMORY -6 +#define CL_PROFILING_INFO_NOT_AVAILABLE -7 +#define CL_MEM_COPY_OVERLAP -8 +#define CL_IMAGE_FORMAT_MISMATCH -9 +#define CL_IMAGE_FORMAT_NOT_SUPPORTED -10 +#define CL_BUILD_PROGRAM_FAILURE -11 +#define CL_MAP_FAILURE -12 +#define CL_MISALIGNED_SUB_BUFFER_OFFSET -13 +#define CL_EXEC_STATUS_ERROR_FOR_EVENTS_IN_WAIT_LIST -14 + +#define CL_INVALID_VALUE -30 +#define CL_INVALID_DEVICE_TYPE -31 +#define CL_INVALID_PLATFORM -32 +#define CL_INVALID_DEVICE -33 +#define CL_INVALID_CONTEXT -34 +#define CL_INVALID_QUEUE_PROPERTIES -35 +#define CL_INVALID_COMMAND_QUEUE -36 +#define CL_INVALID_HOST_PTR -37 +#define CL_INVALID_MEM_OBJECT -38 +#define CL_INVALID_IMAGE_FORMAT_DESCRIPTOR -39 +#define CL_INVALID_IMAGE_SIZE -40 +#define CL_INVALID_SAMPLER -41 +#define CL_INVALID_BINARY -42 +#define CL_INVALID_BUILD_OPTIONS -43 +#define CL_INVALID_PROGRAM -44 +#define CL_INVALID_PROGRAM_EXECUTABLE -45 +#define CL_INVALID_KERNEL_NAME -46 +#define CL_INVALID_KERNEL_DEFINITION -47 +#define CL_INVALID_KERNEL -48 +#define CL_INVALID_ARG_INDEX -49 +#define CL_INVALID_ARG_VALUE -50 +#define CL_INVALID_ARG_SIZE -51 +#define CL_INVALID_KERNEL_ARGS -52 +#define CL_INVALID_WORK_DIMENSION -53 +#define CL_INVALID_WORK_GROUP_SIZE -54 +#define CL_INVALID_WORK_ITEM_SIZE -55 +#define CL_INVALID_GLOBAL_OFFSET -56 +#define CL_INVALID_EVENT_WAIT_LIST -57 +#define CL_INVALID_EVENT -58 +#define CL_INVALID_OPERATION -59 +#define CL_INVALID_GL_OBJECT -60 +#define CL_INVALID_BUFFER_SIZE -61 +#define CL_INVALID_MIP_LEVEL -62 +#define CL_INVALID_GLOBAL_WORK_SIZE -63 +#define CL_INVALID_PROPERTY -64 + +/* OpenCL Version */ +#define CL_VERSION_1_0 1 +#define CL_VERSION_1_1 1 + +/* cl_bool */ +#define CL_FALSE 0 +#define CL_TRUE 1 + +/* cl_platform_info */ +#define CL_PLATFORM_PROFILE 0x0900 +#define CL_PLATFORM_VERSION 0x0901 +#define CL_PLATFORM_NAME 0x0902 +#define CL_PLATFORM_VENDOR 0x0903 +#define CL_PLATFORM_EXTENSIONS 0x0904 + +/* cl_device_type - bitfield */ +#define CL_DEVICE_TYPE_DEFAULT (1 << 0) +#define CL_DEVICE_TYPE_CPU (1 << 1) +#define CL_DEVICE_TYPE_GPU (1 << 2) +#define CL_DEVICE_TYPE_ACCELERATOR (1 << 3) +#define CL_DEVICE_TYPE_ALL 0xFFFFFFFF + +/* cl_device_info */ +#define CL_DEVICE_TYPE 0x1000 +#define CL_DEVICE_VENDOR_ID 0x1001 +#define CL_DEVICE_MAX_COMPUTE_UNITS 0x1002 +#define CL_DEVICE_MAX_WORK_ITEM_DIMENSIONS 0x1003 +#define CL_DEVICE_MAX_WORK_GROUP_SIZE 0x1004 +#define CL_DEVICE_MAX_WORK_ITEM_SIZES 0x1005 +#define CL_DEVICE_PREFERRED_VECTOR_WIDTH_CHAR 0x1006 +#define CL_DEVICE_PREFERRED_VECTOR_WIDTH_SHORT 0x1007 +#define CL_DEVICE_PREFERRED_VECTOR_WIDTH_INT 0x1008 +#define CL_DEVICE_PREFERRED_VECTOR_WIDTH_LONG 0x1009 +#define CL_DEVICE_PREFERRED_VECTOR_WIDTH_FLOAT 0x100A +#define CL_DEVICE_PREFERRED_VECTOR_WIDTH_DOUBLE 0x100B +#define CL_DEVICE_MAX_CLOCK_FREQUENCY 0x100C +#define CL_DEVICE_ADDRESS_BITS 0x100D +#define CL_DEVICE_MAX_READ_IMAGE_ARGS 0x100E +#define CL_DEVICE_MAX_WRITE_IMAGE_ARGS 0x100F +#define CL_DEVICE_MAX_MEM_ALLOC_SIZE 0x1010 +#define CL_DEVICE_IMAGE2D_MAX_WIDTH 0x1011 +#define CL_DEVICE_IMAGE2D_MAX_HEIGHT 0x1012 +#define CL_DEVICE_IMAGE3D_MAX_WIDTH 0x1013 +#define CL_DEVICE_IMAGE3D_MAX_HEIGHT 0x1014 +#define CL_DEVICE_IMAGE3D_MAX_DEPTH 0x1015 +#define CL_DEVICE_IMAGE_SUPPORT 0x1016 +#define CL_DEVICE_MAX_PARAMETER_SIZE 0x1017 +#define CL_DEVICE_MAX_SAMPLERS 0x1018 +#define CL_DEVICE_MEM_BASE_ADDR_ALIGN 0x1019 +#define CL_DEVICE_MIN_DATA_TYPE_ALIGN_SIZE 0x101A +#define CL_DEVICE_SINGLE_FP_CONFIG 0x101B +#define CL_DEVICE_GLOBAL_MEM_CACHE_TYPE 0x101C +#define CL_DEVICE_GLOBAL_MEM_CACHELINE_SIZE 0x101D +#define CL_DEVICE_GLOBAL_MEM_CACHE_SIZE 0x101E +#define CL_DEVICE_GLOBAL_MEM_SIZE 0x101F +#define CL_DEVICE_MAX_CONSTANT_BUFFER_SIZE 0x1020 +#define CL_DEVICE_MAX_CONSTANT_ARGS 0x1021 +#define CL_DEVICE_LOCAL_MEM_TYPE 0x1022 +#define CL_DEVICE_LOCAL_MEM_SIZE 0x1023 +#define CL_DEVICE_ERROR_CORRECTION_SUPPORT 0x1024 +#define CL_DEVICE_PROFILING_TIMER_RESOLUTION 0x1025 +#define CL_DEVICE_ENDIAN_LITTLE 0x1026 +#define CL_DEVICE_AVAILABLE 0x1027 +#define CL_DEVICE_COMPILER_AVAILABLE 0x1028 +#define CL_DEVICE_EXECUTION_CAPABILITIES 0x1029 +#define CL_DEVICE_QUEUE_PROPERTIES 0x102A +#define CL_DEVICE_NAME 0x102B +#define CL_DEVICE_VENDOR 0x102C +#define CL_DRIVER_VERSION 0x102D +#define CL_DEVICE_PROFILE 0x102E +#define CL_DEVICE_VERSION 0x102F +#define CL_DEVICE_EXTENSIONS 0x1030 +#define CL_DEVICE_PLATFORM 0x1031 +/* 0x1032 reserved for CL_DEVICE_DOUBLE_FP_CONFIG */ +/* 0x1033 reserved for CL_DEVICE_HALF_FP_CONFIG */ +#define CL_DEVICE_PREFERRED_VECTOR_WIDTH_HALF 0x1034 +#define CL_DEVICE_HOST_UNIFIED_MEMORY 0x1035 +#define CL_DEVICE_NATIVE_VECTOR_WIDTH_CHAR 0x1036 +#define CL_DEVICE_NATIVE_VECTOR_WIDTH_SHORT 0x1037 +#define CL_DEVICE_NATIVE_VECTOR_WIDTH_INT 0x1038 +#define CL_DEVICE_NATIVE_VECTOR_WIDTH_LONG 0x1039 +#define CL_DEVICE_NATIVE_VECTOR_WIDTH_FLOAT 0x103A +#define CL_DEVICE_NATIVE_VECTOR_WIDTH_DOUBLE 0x103B +#define CL_DEVICE_NATIVE_VECTOR_WIDTH_HALF 0x103C +#define CL_DEVICE_OPENCL_C_VERSION 0x103D + +/* cl_device_fp_config - bitfield */ +#define CL_FP_DENORM (1 << 0) +#define CL_FP_INF_NAN (1 << 1) +#define CL_FP_ROUND_TO_NEAREST (1 << 2) +#define CL_FP_ROUND_TO_ZERO (1 << 3) +#define CL_FP_ROUND_TO_INF (1 << 4) +#define CL_FP_FMA (1 << 5) +#define CL_FP_SOFT_FLOAT (1 << 6) + +/* cl_device_mem_cache_type */ +#define CL_NONE 0x0 +#define CL_READ_ONLY_CACHE 0x1 +#define CL_READ_WRITE_CACHE 0x2 + +/* cl_device_local_mem_type */ +#define CL_LOCAL 0x1 +#define CL_GLOBAL 0x2 + +/* cl_device_exec_capabilities - bitfield */ +#define CL_EXEC_KERNEL (1 << 0) +#define CL_EXEC_NATIVE_KERNEL (1 << 1) + +/* cl_command_queue_properties - bitfield */ +#define CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE (1 << 0) +#define CL_QUEUE_PROFILING_ENABLE (1 << 1) + +/* cl_context_info */ +#define CL_CONTEXT_REFERENCE_COUNT 0x1080 +#define CL_CONTEXT_DEVICES 0x1081 +#define CL_CONTEXT_PROPERTIES 0x1082 +#define CL_CONTEXT_NUM_DEVICES 0x1083 + +/* cl_context_info + cl_context_properties */ +#define CL_CONTEXT_PLATFORM 0x1084 + +/* cl_command_queue_info */ +#define CL_QUEUE_CONTEXT 0x1090 +#define CL_QUEUE_DEVICE 0x1091 +#define CL_QUEUE_REFERENCE_COUNT 0x1092 +#define CL_QUEUE_PROPERTIES 0x1093 + +/* cl_mem_flags - bitfield */ +#define CL_MEM_READ_WRITE (1 << 0) +#define CL_MEM_WRITE_ONLY (1 << 1) +#define CL_MEM_READ_ONLY (1 << 2) +#define CL_MEM_USE_HOST_PTR (1 << 3) +#define CL_MEM_ALLOC_HOST_PTR (1 << 4) +#define CL_MEM_COPY_HOST_PTR (1 << 5) + +/* cl_channel_order */ +#define CL_R 0x10B0 +#define CL_A 0x10B1 +#define CL_RG 0x10B2 +#define CL_RA 0x10B3 +#define CL_RGB 0x10B4 +#define CL_RGBA 0x10B5 +#define CL_BGRA 0x10B6 +#define CL_ARGB 0x10B7 +#define CL_INTENSITY 0x10B8 +#define CL_LUMINANCE 0x10B9 +#define CL_Rx 0x10BA +#define CL_RGx 0x10BB +#define CL_RGBx 0x10BC + +/* cl_channel_type */ +#define CL_SNORM_INT8 0x10D0 +#define CL_SNORM_INT16 0x10D1 +#define CL_UNORM_INT8 0x10D2 +#define CL_UNORM_INT16 0x10D3 +#define CL_UNORM_SHORT_565 0x10D4 +#define CL_UNORM_SHORT_555 0x10D5 +#define CL_UNORM_INT_101010 0x10D6 +#define CL_SIGNED_INT8 0x10D7 +#define CL_SIGNED_INT16 0x10D8 +#define CL_SIGNED_INT32 0x10D9 +#define CL_UNSIGNED_INT8 0x10DA +#define CL_UNSIGNED_INT16 0x10DB +#define CL_UNSIGNED_INT32 0x10DC +#define CL_HALF_FLOAT 0x10DD +#define CL_FLOAT 0x10DE + +/* cl_mem_object_type */ +#define CL_MEM_OBJECT_BUFFER 0x10F0 +#define CL_MEM_OBJECT_IMAGE2D 0x10F1 +#define CL_MEM_OBJECT_IMAGE3D 0x10F2 + +/* cl_mem_info */ +#define CL_MEM_TYPE 0x1100 +#define CL_MEM_FLAGS 0x1101 +#define CL_MEM_SIZE 0x1102 +#define CL_MEM_HOST_PTR 0x1103 +#define CL_MEM_MAP_COUNT 0x1104 +#define CL_MEM_REFERENCE_COUNT 0x1105 +#define CL_MEM_CONTEXT 0x1106 +#define CL_MEM_ASSOCIATED_MEMOBJECT 0x1107 +#define CL_MEM_OFFSET 0x1108 + +/* cl_image_info */ +#define CL_IMAGE_FORMAT 0x1110 +#define CL_IMAGE_ELEMENT_SIZE 0x1111 +#define CL_IMAGE_ROW_PITCH 0x1112 +#define CL_IMAGE_SLICE_PITCH 0x1113 +#define CL_IMAGE_WIDTH 0x1114 +#define CL_IMAGE_HEIGHT 0x1115 +#define CL_IMAGE_DEPTH 0x1116 + +/* cl_addressing_mode */ +#define CL_ADDRESS_NONE 0x1130 +#define CL_ADDRESS_CLAMP_TO_EDGE 0x1131 +#define CL_ADDRESS_CLAMP 0x1132 +#define CL_ADDRESS_REPEAT 0x1133 +#define CL_ADDRESS_MIRRORED_REPEAT 0x1134 + +/* cl_filter_mode */ +#define CL_FILTER_NEAREST 0x1140 +#define CL_FILTER_LINEAR 0x1141 + +/* cl_sampler_info */ +#define CL_SAMPLER_REFERENCE_COUNT 0x1150 +#define CL_SAMPLER_CONTEXT 0x1151 +#define CL_SAMPLER_NORMALIZED_COORDS 0x1152 +#define CL_SAMPLER_ADDRESSING_MODE 0x1153 +#define CL_SAMPLER_FILTER_MODE 0x1154 + +/* cl_map_flags - bitfield */ +#define CL_MAP_READ (1 << 0) +#define CL_MAP_WRITE (1 << 1) + +/* cl_program_info */ +#define CL_PROGRAM_REFERENCE_COUNT 0x1160 +#define CL_PROGRAM_CONTEXT 0x1161 +#define CL_PROGRAM_NUM_DEVICES 0x1162 +#define CL_PROGRAM_DEVICES 0x1163 +#define CL_PROGRAM_SOURCE 0x1164 +#define CL_PROGRAM_BINARY_SIZES 0x1165 +#define CL_PROGRAM_BINARIES 0x1166 + +/* cl_program_build_info */ +#define CL_PROGRAM_BUILD_STATUS 0x1181 +#define CL_PROGRAM_BUILD_OPTIONS 0x1182 +#define CL_PROGRAM_BUILD_LOG 0x1183 + +/* cl_build_status */ +#define CL_BUILD_SUCCESS 0 +#define CL_BUILD_NONE -1 +#define CL_BUILD_ERROR -2 +#define CL_BUILD_IN_PROGRESS -3 + +/* cl_kernel_info */ +#define CL_KERNEL_FUNCTION_NAME 0x1190 +#define CL_KERNEL_NUM_ARGS 0x1191 +#define CL_KERNEL_REFERENCE_COUNT 0x1192 +#define CL_KERNEL_CONTEXT 0x1193 +#define CL_KERNEL_PROGRAM 0x1194 + +/* cl_kernel_work_group_info */ +#define CL_KERNEL_WORK_GROUP_SIZE 0x11B0 +#define CL_KERNEL_COMPILE_WORK_GROUP_SIZE 0x11B1 +#define CL_KERNEL_LOCAL_MEM_SIZE 0x11B2 +#define CL_KERNEL_PREFERRED_WORK_GROUP_SIZE_MULTIPLE 0x11B3 +#define CL_KERNEL_PRIVATE_MEM_SIZE 0x11B4 + +/* cl_event_info */ +#define CL_EVENT_COMMAND_QUEUE 0x11D0 +#define CL_EVENT_COMMAND_TYPE 0x11D1 +#define CL_EVENT_REFERENCE_COUNT 0x11D2 +#define CL_EVENT_COMMAND_EXECUTION_STATUS 0x11D3 +#define CL_EVENT_CONTEXT 0x11D4 + +/* cl_command_type */ +#define CL_COMMAND_NDRANGE_KERNEL 0x11F0 +#define CL_COMMAND_TASK 0x11F1 +#define CL_COMMAND_NATIVE_KERNEL 0x11F2 +#define CL_COMMAND_READ_BUFFER 0x11F3 +#define CL_COMMAND_WRITE_BUFFER 0x11F4 +#define CL_COMMAND_COPY_BUFFER 0x11F5 +#define CL_COMMAND_READ_IMAGE 0x11F6 +#define CL_COMMAND_WRITE_IMAGE 0x11F7 +#define CL_COMMAND_COPY_IMAGE 0x11F8 +#define CL_COMMAND_COPY_IMAGE_TO_BUFFER 0x11F9 +#define CL_COMMAND_COPY_BUFFER_TO_IMAGE 0x11FA +#define CL_COMMAND_MAP_BUFFER 0x11FB +#define CL_COMMAND_MAP_IMAGE 0x11FC +#define CL_COMMAND_UNMAP_MEM_OBJECT 0x11FD +#define CL_COMMAND_MARKER 0x11FE +#define CL_COMMAND_ACQUIRE_GL_OBJECTS 0x11FF +#define CL_COMMAND_RELEASE_GL_OBJECTS 0x1200 +#define CL_COMMAND_READ_BUFFER_RECT 0x1201 +#define CL_COMMAND_WRITE_BUFFER_RECT 0x1202 +#define CL_COMMAND_COPY_BUFFER_RECT 0x1203 +#define CL_COMMAND_USER 0x1204 + +/* command execution status */ +#define CL_COMPLETE 0x0 +#define CL_RUNNING 0x1 +#define CL_SUBMITTED 0x2 +#define CL_QUEUED 0x3 + +/* cl_buffer_create_type */ +#define CL_BUFFER_CREATE_TYPE_REGION 0x1220 + +/* cl_profiling_info */ +#define CL_PROFILING_COMMAND_QUEUED 0x1280 +#define CL_PROFILING_COMMAND_SUBMIT 0x1281 +#define CL_PROFILING_COMMAND_START 0x1282 +#define CL_PROFILING_COMMAND_END 0x1283 + +/********************************************************************************************************/ + +/********************************************************************************************************/ + +/* Function signature typedef's */ + +/* Platform API */ +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETPLATFORMIDS)(cl_uint /* num_entries */, + cl_platform_id * /* platforms */, + cl_uint * /* num_platforms */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETPLATFORMINFO)(cl_platform_id /* platform */, + cl_platform_info /* param_name */, + size_t /* param_value_size */, + void * /* param_value */, + size_t * /* param_value_size_ret */) CL_API_SUFFIX__VERSION_1_0; + +/* Device APIs */ +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETDEVICEIDS)(cl_platform_id /* platform */, + cl_device_type /* device_type */, + cl_uint /* num_entries */, + cl_device_id * /* devices */, + cl_uint * /* num_devices */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETDEVICEINFO)(cl_device_id /* device */, + cl_device_info /* param_name */, + size_t /* param_value_size */, + void * /* param_value */, + size_t * /* param_value_size_ret */) CL_API_SUFFIX__VERSION_1_0; + +// Context APIs +typedef CL_API_ENTRY cl_context (CL_API_CALL * +PFNCLCREATECONTEXT)(const cl_context_properties * /* properties */, + cl_uint /* num_devices */, + const cl_device_id * /* devices */, + void (CL_CALLBACK * /* pfn_notify */)(const char *, const void *, size_t, void *), + void * /* user_data */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_context (CL_API_CALL * +PFNCLCREATECONTEXTFROMTYPE)(const cl_context_properties * /* properties */, + cl_device_type /* device_type */, + void (CL_CALLBACK * /* pfn_notify*/ )(const char *, const void *, size_t, void *), + void * /* user_data */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRETAINCONTEXT)(cl_context /* context */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRELEASECONTEXT)(cl_context /* context */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETCONTEXTINFO)(cl_context /* context */, + cl_context_info /* param_name */, + size_t /* param_value_size */, + void * /* param_value */, + size_t * /* param_value_size_ret */) CL_API_SUFFIX__VERSION_1_0; + +/* Command Queue APIs */ +typedef CL_API_ENTRY cl_command_queue (CL_API_CALL * +PFNCLCREATECOMMANDQUEUE)(cl_context /* context */, + cl_device_id /* device */, + cl_command_queue_properties /* properties */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRETAINCOMMANDQUEUE)(cl_command_queue /* command_queue */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRELEASECOMMANDQUEUE)(cl_command_queue /* command_queue */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETCOMMANDQUEUEINFO)(cl_command_queue /* command_queue */, + cl_command_queue_info /* param_name */, + size_t /* param_value_size */, + void * /* param_value */, + size_t * /* param_value_size_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLSETCOMMANDQUEUEPROPERTY)(cl_command_queue /* command_queue */, + cl_command_queue_properties /* properties */, + cl_bool /* enable */, + cl_command_queue_properties * /* old_properties */) CL_API_SUFFIX__VERSION_1_0; + +/* Memory Object APIs */ +typedef CL_API_ENTRY cl_mem (CL_API_CALL * +PFNCLCREATEBUFFER)(cl_context /* context */, + cl_mem_flags /* flags */, + size_t /* size */, + void * /* host_ptr */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_mem (CL_API_CALL * +PFNCLCREATESUBBUFFER)(cl_mem /* buffer */, + cl_mem_flags /* flags */, + cl_buffer_create_type /* buffer_create_type */, + const void * /* buffer_create_info */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_1; + +typedef CL_API_ENTRY cl_mem (CL_API_CALL * +PFNCLCREATEIMAGE2D)(cl_context /* context */, + cl_mem_flags /* flags */, + const cl_image_format * /* image_format */, + size_t /* image_width */, + size_t /* image_height */, + size_t /* image_row_pitch */, + void * /* host_ptr */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_mem (CL_API_CALL * +PFNCLCREATEIMAGE3D)(cl_context /* context */, + cl_mem_flags /* flags */, + const cl_image_format * /* image_format */, + size_t /* image_width */, + size_t /* image_height */, + size_t /* image_depth */, + size_t /* image_row_pitch */, + size_t /* image_slice_pitch */, + void * /* host_ptr */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRETAINMEMOBJECT)(cl_mem /* memobj */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRELEASEMEMOBJECT)(cl_mem /* memobj */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETSUPPORTEDIMAGEFORMATS)(cl_context /* context */, + cl_mem_flags /* flags */, + cl_mem_object_type /* image_type */, + cl_uint /* num_entries */, + cl_image_format * /* image_formats */, + cl_uint * /* num_image_formats */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETMEMOBJECTINFO)(cl_mem /* memobj */, + cl_mem_info /* param_name */, + size_t /* param_value_size */, + void * /* param_value */, + size_t * /* param_value_size_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETIMAGEINFO)(cl_mem /* image */, + cl_image_info /* param_name */, + size_t /* param_value_size */, + void * /* param_value */, + size_t * /* param_value_size_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLSETMEMOBJECTDESTRUCTORCALLBACK)( cl_mem /* memobj */, + void (CL_CALLBACK * /*pfn_notify*/)( cl_mem /* memobj */, void* /*user_data*/), + void * /*user_data */ ) CL_API_SUFFIX__VERSION_1_1; + +/* Sampler APIs */ +typedef CL_API_ENTRY cl_sampler (CL_API_CALL * +PFNCLCREATESAMPLER)(cl_context /* context */, + cl_bool /* normalized_coords */, + cl_addressing_mode /* addressing_mode */, + cl_filter_mode /* filter_mode */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRETAINSAMPLER)(cl_sampler /* sampler */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRELEASESAMPLER)(cl_sampler /* sampler */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETSAMPLERINFO)(cl_sampler /* sampler */, + cl_sampler_info /* param_name */, + size_t /* param_value_size */, + void * /* param_value */, + size_t * /* param_value_size_ret */) CL_API_SUFFIX__VERSION_1_0; + +/* Program Object APIs */ +typedef CL_API_ENTRY cl_program (CL_API_CALL * +PFNCLCREATEPROGRAMWITHSOURCE)(cl_context /* context */, + cl_uint /* count */, + const char ** /* strings */, + const size_t * /* lengths */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_program (CL_API_CALL * +PFNCLCREATEPROGRAMWITHBINARY)(cl_context /* context */, + cl_uint /* num_devices */, + const cl_device_id * /* device_list */, + const size_t * /* lengths */, + const unsigned char ** /* binaries */, + cl_int * /* binary_status */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRETAINPROGRAM)(cl_program /* program */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRELEASEPROGRAM)(cl_program /* program */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLBUILDPROGRAM)(cl_program /* program */, + cl_uint /* num_devices */, + const cl_device_id * /* device_list */, + const char * /* options */, + void (CL_CALLBACK * /* pfn_notify */)(cl_program /* program */, void * /* user_data */), + void * /* user_data */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLUNLOADCOMPILER)(void) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETPROGRAMINFO)(cl_program /* program */, + cl_program_info /* param_name */, + size_t /* param_value_size */, + void * /* param_value */, + size_t * /* param_value_size_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETPROGRAMBUILDINFO)(cl_program /* program */, + cl_device_id /* device */, + cl_program_build_info /* param_name */, + size_t /* param_value_size */, + void * /* param_value */, + size_t * /* param_value_size_ret */) CL_API_SUFFIX__VERSION_1_0; + +/* Kernel Object APIs */ +typedef CL_API_ENTRY cl_kernel (CL_API_CALL * +PFNCLCREATEKERNEL)(cl_program /* program */, + const char * /* kernel_name */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLCREATEKERNELSINPROGRAM)(cl_program /* program */, + cl_uint /* num_kernels */, + cl_kernel * /* kernels */, + cl_uint * /* num_kernels_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRETAINKERNEL)(cl_kernel /* kernel */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRELEASEKERNEL)(cl_kernel /* kernel */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLSETKERNELARG)(cl_kernel /* kernel */, + cl_uint /* arg_index */, + size_t /* arg_size */, + const void * /* arg_value */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETKERNELINFO)(cl_kernel /* kernel */, + cl_kernel_info /* param_name */, + size_t /* param_value_size */, + void * /* param_value */, + size_t * /* param_value_size_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETKERNELWORKGROUPINFO)(cl_kernel /* kernel */, + cl_device_id /* device */, + cl_kernel_work_group_info /* param_name */, + size_t /* param_value_size */, + void * /* param_value */, + size_t * /* param_value_size_ret */) CL_API_SUFFIX__VERSION_1_0; + +// Event Object APIs +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLWAITFOREVENTS)(cl_uint /* num_events */, + const cl_event * /* event_list */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETEVENTINFO)(cl_event /* event */, + cl_event_info /* param_name */, + size_t /* param_value_size */, + void * /* param_value */, + size_t * /* param_value_size_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_event (CL_API_CALL * +PFNCLCREATEUSEREVENT)(cl_context /* context */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_1; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRETAINEVENT)(cl_event /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLRELEASEEVENT)(cl_event /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLSETUSEREVENTSTATUS)(cl_event /* event */, + cl_int /* execution_status */) CL_API_SUFFIX__VERSION_1_1; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLSETEVENTCALLBACK)( cl_event /* event */, + cl_int /* command_exec_callback_type */, + void (CL_CALLBACK * /* pfn_notify */)(cl_event, cl_int, void *), + void * /* user_data */) CL_API_SUFFIX__VERSION_1_1; + +/* Profiling APIs */ +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLGETEVENTPROFILINGINFO)(cl_event /* event */, + cl_profiling_info /* param_name */, + size_t /* param_value_size */, + void * /* param_value */, + size_t * /* param_value_size_ret */) CL_API_SUFFIX__VERSION_1_0; + +// Flush and Finish APIs +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLFLUSH)(cl_command_queue /* command_queue */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLFINISH)(cl_command_queue /* command_queue */) CL_API_SUFFIX__VERSION_1_0; + +/* Enqueued Commands APIs */ +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUEREADBUFFER)(cl_command_queue /* command_queue */, + cl_mem /* buffer */, + cl_bool /* blocking_read */, + size_t /* offset */, + size_t /* cb */, + void * /* ptr */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUEREADBUFFERRECT)(cl_command_queue /* command_queue */, + cl_mem /* buffer */, + cl_bool /* blocking_read */, + const size_t * /* buffer_origin */, + const size_t * /* host_origin */, + const size_t * /* region */, + size_t /* buffer_row_pitch */, + size_t /* buffer_slice_pitch */, + size_t /* host_row_pitch */, + size_t /* host_slice_pitch */, + void * /* ptr */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_1; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUEWRITEBUFFER)(cl_command_queue /* command_queue */, + cl_mem /* buffer */, + cl_bool /* blocking_write */, + size_t /* offset */, + size_t /* cb */, + const void * /* ptr */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUEWRITEBUFFERRECT)(cl_command_queue /* command_queue */, + cl_mem /* buffer */, + cl_bool /* blocking_write */, + const size_t * /* buffer_origin */, + const size_t * /* host_origin */, + const size_t * /* region */, + size_t /* buffer_row_pitch */, + size_t /* buffer_slice_pitch */, + size_t /* host_row_pitch */, + size_t /* host_slice_pitch */, + const void * /* ptr */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_1; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUECOPYBUFFER)(cl_command_queue /* command_queue */, + cl_mem /* src_buffer */, + cl_mem /* dst_buffer */, + size_t /* src_offset */, + size_t /* dst_offset */, + size_t /* cb */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUECOPYBUFFERRECT)(cl_command_queue /* command_queue */, + cl_mem /* src_buffer */, + cl_mem /* dst_buffer */, + const size_t * /* src_origin */, + const size_t * /* dst_origin */, + const size_t * /* region */, + size_t /* src_row_pitch */, + size_t /* src_slice_pitch */, + size_t /* dst_row_pitch */, + size_t /* dst_slice_pitch */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_1; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUEREADIMAGE)(cl_command_queue /* command_queue */, + cl_mem /* image */, + cl_bool /* blocking_read */, + const size_t * /* origin[3] */, + const size_t * /* region[3] */, + size_t /* row_pitch */, + size_t /* slice_pitch */, + void * /* ptr */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUEWRITEIMAGE)(cl_command_queue /* command_queue */, + cl_mem /* image */, + cl_bool /* blocking_write */, + const size_t * /* origin[3] */, + const size_t * /* region[3] */, + size_t /* input_row_pitch */, + size_t /* input_slice_pitch */, + const void * /* ptr */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUECOPYIMAGE)(cl_command_queue /* command_queue */, + cl_mem /* src_image */, + cl_mem /* dst_image */, + const size_t * /* src_origin[3] */, + const size_t * /* dst_origin[3] */, + const size_t * /* region[3] */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUECOPYIMAGETOBUFFER)(cl_command_queue /* command_queue */, + cl_mem /* src_image */, + cl_mem /* dst_buffer */, + const size_t * /* src_origin[3] */, + const size_t * /* region[3] */, + size_t /* dst_offset */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUECOPYBUFFERTOIMAGE)(cl_command_queue /* command_queue */, + cl_mem /* src_buffer */, + cl_mem /* dst_image */, + size_t /* src_offset */, + const size_t * /* dst_origin[3] */, + const size_t * /* region[3] */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY void * (CL_API_CALL * +PFNCLENQUEUEMAPBUFFER)(cl_command_queue /* command_queue */, + cl_mem /* buffer */, + cl_bool /* blocking_map */, + cl_map_flags /* map_flags */, + size_t /* offset */, + size_t /* cb */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY void * (CL_API_CALL * +PFNCLENQUEUEMAPIMAGE)(cl_command_queue /* command_queue */, + cl_mem /* image */, + cl_bool /* blocking_map */, + cl_map_flags /* map_flags */, + const size_t * /* origin[3] */, + const size_t * /* region[3] */, + size_t * /* image_row_pitch */, + size_t * /* image_slice_pitch */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */, + cl_int * /* errcode_ret */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUEUNMAPMEMOBJECT)(cl_command_queue /* command_queue */, + cl_mem /* memobj */, + void * /* mapped_ptr */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUENDRANGEKERNEL)(cl_command_queue /* command_queue */, + cl_kernel /* kernel */, + cl_uint /* work_dim */, + const size_t * /* global_work_offset */, + const size_t * /* global_work_size */, + const size_t * /* local_work_size */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUETASK)(cl_command_queue /* command_queue */, + cl_kernel /* kernel */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUENATIVEKERNEL)(cl_command_queue /* command_queue */, + void (*user_func)(void *), + void * /* args */, + size_t /* cb_args */, + cl_uint /* num_mem_objects */, + const cl_mem * /* mem_list */, + const void ** /* args_mem_loc */, + cl_uint /* num_events_in_wait_list */, + const cl_event * /* event_wait_list */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUEMARKER)(cl_command_queue /* command_queue */, + cl_event * /* event */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUEWAITFOREVENTS)(cl_command_queue /* command_queue */, + cl_uint /* num_events */, + const cl_event * /* event_list */) CL_API_SUFFIX__VERSION_1_0; + +typedef CL_API_ENTRY cl_int (CL_API_CALL * +PFNCLENQUEUEBARRIER)(cl_command_queue /* command_queue */) CL_API_SUFFIX__VERSION_1_0; + +// Extension function access +// +// Returns the extension function address for the given function name, +// or NULL if a valid function can not be found. The client must +// check to make sure the address is not NULL, before using or +// calling the returned function address. +// +typedef CL_API_ENTRY void * (CL_API_CALL * PFNCLGETEXTENSIONFUNCTIONADDRESS)(const char * /* func_name */) CL_API_SUFFIX__VERSION_1_0; + + +#define CLEW_STATIC + +#ifdef CLEW_STATIC +# define CLEWAPI extern +#else +# ifdef CLEW_BUILD +# define CLEWAPI extern __declspec(dllexport) +# else +# define CLEWAPI extern __declspec(dllimport) +# endif +#endif + +#if defined(_WIN32) +#define CLEW_FUN_EXPORT extern +#else +#define CLEW_FUN_EXPORT CLEWAPI +#endif + +#define CLEW_GET_FUN(x) x + + +// Variables holding function entry points +CLEW_FUN_EXPORT PFNCLGETPLATFORMIDS __clewGetPlatformIDs ; +CLEW_FUN_EXPORT PFNCLGETPLATFORMINFO __clewGetPlatformInfo ; +CLEW_FUN_EXPORT PFNCLGETDEVICEIDS __clewGetDeviceIDs ; +CLEW_FUN_EXPORT PFNCLGETDEVICEINFO __clewGetDeviceInfo ; +CLEW_FUN_EXPORT PFNCLCREATECONTEXT __clewCreateContext ; +CLEW_FUN_EXPORT PFNCLCREATECONTEXTFROMTYPE __clewCreateContextFromType ; +CLEW_FUN_EXPORT PFNCLRETAINCONTEXT __clewRetainContext ; +CLEW_FUN_EXPORT PFNCLRELEASECONTEXT __clewReleaseContext ; +CLEW_FUN_EXPORT PFNCLGETCONTEXTINFO __clewGetContextInfo ; +CLEW_FUN_EXPORT PFNCLCREATECOMMANDQUEUE __clewCreateCommandQueue ; +CLEW_FUN_EXPORT PFNCLRETAINCOMMANDQUEUE __clewRetainCommandQueue ; +CLEW_FUN_EXPORT PFNCLRELEASECOMMANDQUEUE __clewReleaseCommandQueue ; +CLEW_FUN_EXPORT PFNCLGETCOMMANDQUEUEINFO __clewGetCommandQueueInfo ; +#ifdef CL_USE_DEPRECATED_OPENCL_1_0_APIS +CLEW_FUN_EXPORT PFNCLSETCOMMANDQUEUEPROPERTY __clewSetCommandQueueProperty ; +#endif +CLEW_FUN_EXPORT PFNCLCREATEBUFFER __clewCreateBuffer ; +CLEW_FUN_EXPORT PFNCLCREATESUBBUFFER __clewCreateSubBuffer ; +CLEW_FUN_EXPORT PFNCLCREATEIMAGE2D __clewCreateImage2D ; +CLEW_FUN_EXPORT PFNCLCREATEIMAGE3D __clewCreateImage3D ; +CLEW_FUN_EXPORT PFNCLRETAINMEMOBJECT __clewRetainMemObject ; +CLEW_FUN_EXPORT PFNCLRELEASEMEMOBJECT __clewReleaseMemObject ; +CLEW_FUN_EXPORT PFNCLGETSUPPORTEDIMAGEFORMATS __clewGetSupportedImageFormats ; +CLEW_FUN_EXPORT PFNCLGETMEMOBJECTINFO __clewGetMemObjectInfo ; +CLEW_FUN_EXPORT PFNCLGETIMAGEINFO __clewGetImageInfo ; +CLEW_FUN_EXPORT PFNCLSETMEMOBJECTDESTRUCTORCALLBACK __clewSetMemObjectDestructorCallback; +CLEW_FUN_EXPORT PFNCLCREATESAMPLER __clewCreateSampler ; +CLEW_FUN_EXPORT PFNCLRETAINSAMPLER __clewRetainSampler ; +CLEW_FUN_EXPORT PFNCLRELEASESAMPLER __clewReleaseSampler ; +CLEW_FUN_EXPORT PFNCLGETSAMPLERINFO __clewGetSamplerInfo ; +CLEW_FUN_EXPORT PFNCLCREATEPROGRAMWITHSOURCE __clewCreateProgramWithSource ; +CLEW_FUN_EXPORT PFNCLCREATEPROGRAMWITHBINARY __clewCreateProgramWithBinary ; +CLEW_FUN_EXPORT PFNCLRETAINPROGRAM __clewRetainProgram ; +CLEW_FUN_EXPORT PFNCLRELEASEPROGRAM __clewReleaseProgram ; +CLEW_FUN_EXPORT PFNCLBUILDPROGRAM __clewBuildProgram ; +CLEW_FUN_EXPORT PFNCLUNLOADCOMPILER __clewUnloadCompiler ; +CLEW_FUN_EXPORT PFNCLGETPROGRAMINFO __clewGetProgramInfo ; +CLEW_FUN_EXPORT PFNCLGETPROGRAMBUILDINFO __clewGetProgramBuildInfo ; +CLEW_FUN_EXPORT PFNCLCREATEKERNEL __clewCreateKernel ; +CLEW_FUN_EXPORT PFNCLCREATEKERNELSINPROGRAM __clewCreateKernelsInProgram ; +CLEW_FUN_EXPORT PFNCLRETAINKERNEL __clewRetainKernel ; +CLEW_FUN_EXPORT PFNCLRELEASEKERNEL __clewReleaseKernel ; +CLEW_FUN_EXPORT PFNCLSETKERNELARG __clewSetKernelArg ; +CLEW_FUN_EXPORT PFNCLGETKERNELINFO __clewGetKernelInfo ; +CLEW_FUN_EXPORT PFNCLGETKERNELWORKGROUPINFO __clewGetKernelWorkGroupInfo ; +CLEW_FUN_EXPORT PFNCLWAITFOREVENTS __clewWaitForEvents ; +CLEW_FUN_EXPORT PFNCLGETEVENTINFO __clewGetEventInfo ; +CLEW_FUN_EXPORT PFNCLCREATEUSEREVENT __clewCreateUserEvent ; +CLEW_FUN_EXPORT PFNCLRETAINEVENT __clewRetainEvent ; +CLEW_FUN_EXPORT PFNCLRELEASEEVENT __clewReleaseEvent ; +CLEW_FUN_EXPORT PFNCLSETUSEREVENTSTATUS __clewSetUserEventStatus ; +CLEW_FUN_EXPORT PFNCLSETEVENTCALLBACK __clewSetEventCallback ; +CLEW_FUN_EXPORT PFNCLGETEVENTPROFILINGINFO __clewGetEventProfilingInfo ; +CLEW_FUN_EXPORT PFNCLFLUSH __clewFlush ; +CLEW_FUN_EXPORT PFNCLFINISH __clewFinish ; +CLEW_FUN_EXPORT PFNCLENQUEUEREADBUFFER __clewEnqueueReadBuffer ; +CLEW_FUN_EXPORT PFNCLENQUEUEREADBUFFERRECT __clewEnqueueReadBufferRect ; +CLEW_FUN_EXPORT PFNCLENQUEUEWRITEBUFFER __clewEnqueueWriteBuffer ; +CLEW_FUN_EXPORT PFNCLENQUEUEWRITEBUFFERRECT __clewEnqueueWriteBufferRect ; +CLEW_FUN_EXPORT PFNCLENQUEUECOPYBUFFER __clewEnqueueCopyBuffer ; +CLEW_FUN_EXPORT PFNCLENQUEUECOPYBUFFERRECT __clewEnqueueCopyBufferRect ; +CLEW_FUN_EXPORT PFNCLENQUEUEREADIMAGE __clewEnqueueReadImage ; +CLEW_FUN_EXPORT PFNCLENQUEUEWRITEIMAGE __clewEnqueueWriteImage ; +CLEW_FUN_EXPORT PFNCLENQUEUECOPYIMAGE __clewEnqueueCopyImage ; +CLEW_FUN_EXPORT PFNCLENQUEUECOPYIMAGETOBUFFER __clewEnqueueCopyImageToBuffer ; +CLEW_FUN_EXPORT PFNCLENQUEUECOPYBUFFERTOIMAGE __clewEnqueueCopyBufferToImage ; +CLEW_FUN_EXPORT PFNCLENQUEUEMAPBUFFER __clewEnqueueMapBuffer ; +CLEW_FUN_EXPORT PFNCLENQUEUEMAPIMAGE __clewEnqueueMapImage ; +CLEW_FUN_EXPORT PFNCLENQUEUEUNMAPMEMOBJECT __clewEnqueueUnmapMemObject ; +CLEW_FUN_EXPORT PFNCLENQUEUENDRANGEKERNEL __clewEnqueueNDRangeKernel ; +CLEW_FUN_EXPORT PFNCLENQUEUETASK __clewEnqueueTask ; +CLEW_FUN_EXPORT PFNCLENQUEUENATIVEKERNEL __clewEnqueueNativeKernel ; +CLEW_FUN_EXPORT PFNCLENQUEUEMARKER __clewEnqueueMarker ; +CLEW_FUN_EXPORT PFNCLENQUEUEWAITFOREVENTS __clewEnqueueWaitForEvents ; +CLEW_FUN_EXPORT PFNCLENQUEUEBARRIER __clewEnqueueBarrier ; +CLEW_FUN_EXPORT PFNCLGETEXTENSIONFUNCTIONADDRESS __clewGetExtensionFunctionAddress ; + + +#define clGetPlatformIDs CLEW_GET_FUN(__clewGetPlatformIDs ) +#define clGetPlatformInfo CLEW_GET_FUN(__clewGetPlatformInfo ) +#define clGetDeviceIDs CLEW_GET_FUN(__clewGetDeviceIDs ) +#define clGetDeviceInfo CLEW_GET_FUN(__clewGetDeviceInfo ) +#define clCreateContext CLEW_GET_FUN(__clewCreateContext ) +#define clCreateContextFromType CLEW_GET_FUN(__clewCreateContextFromType ) +#define clRetainContext CLEW_GET_FUN(__clewRetainContext ) +#define clReleaseContext CLEW_GET_FUN(__clewReleaseContext ) +#define clGetContextInfo CLEW_GET_FUN(__clewGetContextInfo ) +#define clCreateCommandQueue CLEW_GET_FUN(__clewCreateCommandQueue ) +#define clRetainCommandQueue CLEW_GET_FUN(__clewRetainCommandQueue ) +#define clReleaseCommandQueue CLEW_GET_FUN(__clewReleaseCommandQueue ) +#define clGetCommandQueueInfo CLEW_GET_FUN(__clewGetCommandQueueInfo ) +#ifdef CL_USE_DEPRECATED_OPENCL_1_0_APIS +#warning CL_USE_DEPRECATED_OPENCL_1_0_APIS is defined. These APIs are unsupported and untested in OpenCL 1.1! +/* + * WARNING: + * This API introduces mutable state into the OpenCL implementation. It has been REMOVED + * to better facilitate thread safety. The 1.0 API is not thread safe. It is not tested by the + * OpenCL 1.1 conformance test, and consequently may not work or may not work dependably. + * It is likely to be non-performant. Use of this API is not advised. Use at your own risk. + * + * Software developers previously relying on this API are instructed to set the command queue + * properties when creating the queue, instead. + */ +#define clSetCommandQueueProperty CLEW_GET_FUN(__clewSetCommandQueueProperty ) +#endif /* CL_USE_DEPRECATED_OPENCL_1_0_APIS */ +#define clCreateBuffer CLEW_GET_FUN(__clewCreateBuffer ) +#define clCreateSubBuffer CLEW_GET_FUN(__clewCreateSubBuffer ) +#define clCreateImage2D CLEW_GET_FUN(__clewCreateImage2D ) +#define clCreateImage3D CLEW_GET_FUN(__clewCreateImage3D ) +#define clRetainMemObject CLEW_GET_FUN(__clewRetainMemObject ) +#define clReleaseMemObject CLEW_GET_FUN(__clewReleaseMemObject ) +#define clGetSupportedImageFormats CLEW_GET_FUN(__clewGetSupportedImageFormats ) +#define clGetMemObjectInfo CLEW_GET_FUN(__clewGetMemObjectInfo ) +#define clGetImageInfo CLEW_GET_FUN(__clewGetImageInfo ) +#define clSetMemObjectDestructorCallback CLEW_GET_FUN(__clewSetMemObjectDestructorCallback) +#define clCreateSampler CLEW_GET_FUN(__clewCreateSampler ) +#define clRetainSampler CLEW_GET_FUN(__clewRetainSampler ) +#define clReleaseSampler CLEW_GET_FUN(__clewReleaseSampler ) +#define clGetSamplerInfo CLEW_GET_FUN(__clewGetSamplerInfo ) +#define clCreateProgramWithSource CLEW_GET_FUN(__clewCreateProgramWithSource ) +#define clCreateProgramWithBinary CLEW_GET_FUN(__clewCreateProgramWithBinary ) +#define clRetainProgram CLEW_GET_FUN(__clewRetainProgram ) +#define clReleaseProgram CLEW_GET_FUN(__clewReleaseProgram ) +#define clBuildProgram CLEW_GET_FUN(__clewBuildProgram ) +#define clUnloadCompiler CLEW_GET_FUN(__clewUnloadCompiler ) +#define clGetProgramInfo CLEW_GET_FUN(__clewGetProgramInfo ) +#define clGetProgramBuildInfo CLEW_GET_FUN(__clewGetProgramBuildInfo ) +#define clCreateKernel CLEW_GET_FUN(__clewCreateKernel ) +#define clCreateKernelsInProgram CLEW_GET_FUN(__clewCreateKernelsInProgram ) +#define clRetainKernel CLEW_GET_FUN(__clewRetainKernel ) +#define clReleaseKernel CLEW_GET_FUN(__clewReleaseKernel ) +#define clSetKernelArg CLEW_GET_FUN(__clewSetKernelArg ) +#define clGetKernelInfo CLEW_GET_FUN(__clewGetKernelInfo ) +#define clGetKernelWorkGroupInfo CLEW_GET_FUN(__clewGetKernelWorkGroupInfo ) +#define clWaitForEvents CLEW_GET_FUN(__clewWaitForEvents ) +#define clGetEventInfo CLEW_GET_FUN(__clewGetEventInfo ) +#define clCreateUserEvent CLEW_GET_FUN(__clewCreateUserEvent ) +#define clRetainEvent CLEW_GET_FUN(__clewRetainEvent ) +#define clReleaseEvent CLEW_GET_FUN(__clewReleaseEvent ) +#define clSetUserEventStatus CLEW_GET_FUN(__clewSetUserEventStatus ) +#define clSetEventCallback CLEW_GET_FUN(__clewSetEventCallback ) +#define clGetEventProfilingInfo CLEW_GET_FUN(__clewGetEventProfilingInfo ) +#define clFlush CLEW_GET_FUN(__clewFlush ) +#define clFinish CLEW_GET_FUN(__clewFinish ) +#define clEnqueueReadBuffer CLEW_GET_FUN(__clewEnqueueReadBuffer ) +#define clEnqueueReadBufferRect CLEW_GET_FUN(__clewEnqueueReadBufferRect ) +#define clEnqueueWriteBuffer CLEW_GET_FUN(__clewEnqueueWriteBuffer ) +#define clEnqueueWriteBufferRect CLEW_GET_FUN(__clewEnqueueWriteBufferRect ) +#define clEnqueueCopyBuffer CLEW_GET_FUN(__clewEnqueueCopyBuffer ) +#define clEnqueueCopyBufferRect CLEW_GET_FUN(__clewEnqueueCopyBufferRect ) +#define clEnqueueReadImage CLEW_GET_FUN(__clewEnqueueReadImage ) +#define clEnqueueWriteImage CLEW_GET_FUN(__clewEnqueueWriteImage ) +#define clEnqueueCopyImage CLEW_GET_FUN(__clewEnqueueCopyImage ) +#define clEnqueueCopyImageToBuffer CLEW_GET_FUN(__clewEnqueueCopyImageToBuffer ) +#define clEnqueueCopyBufferToImage CLEW_GET_FUN(__clewEnqueueCopyBufferToImage ) +#define clEnqueueMapBuffer CLEW_GET_FUN(__clewEnqueueMapBuffer ) +#define clEnqueueMapImage CLEW_GET_FUN(__clewEnqueueMapImage ) +#define clEnqueueUnmapMemObject CLEW_GET_FUN(__clewEnqueueUnmapMemObject ) +#define clEnqueueNDRangeKernel CLEW_GET_FUN(__clewEnqueueNDRangeKernel ) +#define clEnqueueTask CLEW_GET_FUN(__clewEnqueueTask ) +#define clEnqueueNativeKernel CLEW_GET_FUN(__clewEnqueueNativeKernel ) +#define clEnqueueMarker CLEW_GET_FUN(__clewEnqueueMarker ) +#define clEnqueueWaitForEvents CLEW_GET_FUN(__clewEnqueueWaitForEvents ) +#define clEnqueueBarrier CLEW_GET_FUN(__clewEnqueueBarrier ) +#define clGetExtensionFunctionAddress CLEW_GET_FUN(__clewGetExtensionFunctionAddress ) + + +#define CLEW_SUCCESS 0 //!< Success error code +#define CLEW_ERROR_OPEN_FAILED -1 //!< Error code for failing to open the dynamic library +#define CLEW_ERROR_ATEXIT_FAILED -2 //!< Error code for failing to queue the closing of the dynamic library to atexit() + +//! \brief Load OpenCL dynamic library and set function entry points +int clewInit (const char*); + +//! \brief Exit clew and unload OpenCL dynamic library +void clewExit(); + +//! \brief Convert an OpenCL error code to its string equivalent +const char* clewErrorString (cl_int error); + +#ifdef __cplusplus +} +#endif + +#endif // CLEW_HPP_INCLUDED diff --git a/extern/bullet2/readme.txt b/extern/bullet2/readme.txt deleted file mode 100644 index ec99abf71cfd..000000000000 --- a/extern/bullet2/readme.txt +++ /dev/null @@ -1,13 +0,0 @@ -This is the new refactored version of Bullet physics library version 2.x - -Questions? mail blender at erwincoumans.com, or check the bf-blender mailing list. -Thanks, -Erwin - -Apply patches/blender.patch to fix a few build errors and warnings and dd original -vertex access for BMesh convex hull operator. - -Documentation is available at: -http://code.google.com/p/bullet/source/browse/trunk/Bullet_User_Manual.pdf -and: -https://github.com/bulletphysics/bullet3/tree/master/docs diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp deleted file mode 100644 index 81369fe9b50a..000000000000 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp +++ /dev/null @@ -1,489 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btMultiSapBroadphase.h" - -#include "btSimpleBroadphase.h" -#include "LinearMath/btAabbUtil2.h" -#include "btQuantizedBvh.h" - -/// btSapBroadphaseArray m_sapBroadphases; - -/// btOverlappingPairCache* m_overlappingPairs; -extern int gOverlappingPairs; - -/* -class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache -{ -public: - - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) - { - return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy); - } -}; - -*/ - -btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache) -:m_overlappingPairs(pairCache), -m_optimizedAabbTree(0), -m_ownsPairCache(false), -m_invalidPair(0) -{ - if (!m_overlappingPairs) - { - m_ownsPairCache = true; - void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16); - m_overlappingPairs = new (mem)btSortedOverlappingPairCache(); - } - - struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback - { - virtual ~btMultiSapOverlapFilterCallback() - {} - // return true when pairs need collision - virtual bool needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const - { - btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy; - btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy; - - bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0; - collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask); - - return collides; - } - }; - - void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16); - m_filterCallback = new (mem)btMultiSapOverlapFilterCallback(); - - m_overlappingPairs->setOverlapFilterCallback(m_filterCallback); -// mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16); -// m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs); -} - -btMultiSapBroadphase::~btMultiSapBroadphase() -{ - if (m_ownsPairCache) - { - m_overlappingPairs->~btOverlappingPairCache(); - btAlignedFree(m_overlappingPairs); - } -} - - -void btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax) -{ - m_optimizedAabbTree = new btQuantizedBvh(); - m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax); - QuantizedNodeArray& nodes = m_optimizedAabbTree->getLeafNodeArray(); - for (int i=0;igetBroadphaseAabb(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(proxy); - aabbMin = multiProxy->m_aabbMin; - aabbMax = multiProxy->m_aabbMax; -} - -void btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax) -{ - for (int i=0;i - -void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher) -{ - btMultiSapProxy* multiProxy = static_cast(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;im_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;im_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;igetBroadphaseAabb(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;im_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;im_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 ) const - { - 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;im_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;iprintStats(); - - } - */ - -} - -void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher) -{ - // not yet -} diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h deleted file mode 100644 index 7bcfe6b132a8..000000000000 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h +++ /dev/null @@ -1,151 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#ifndef BT_MULTI_SAP_BROADPHASE -#define BT_MULTI_SAP_BROADPHASE - -#include "btBroadphaseInterface.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "btOverlappingPairCache.h" - - -class btBroadphaseInterface; -class btSimpleBroadphase; - - -typedef btAlignedObjectArray btSapBroadphaseArray; - -///The btMultiSapBroadphase is a research project, not recommended to use in production. Use btAxisSweep3 or btDbvtBroadphase instead. -///The btMultiSapBroadphase is a broadphase that contains multiple SAP broadphases. -///The user can add SAP broadphases that cover the world. A btBroadphaseProxy can be in multiple child broadphases at the same time. -///A btQuantizedBvh acceleration structures finds overlapping SAPs for each btBroadphaseProxy. -///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328 -///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329 -class btMultiSapBroadphase :public btBroadphaseInterface -{ - btSapBroadphaseArray m_sapBroadphases; - - btSimpleBroadphase* m_simpleBroadphase; - - btOverlappingPairCache* m_overlappingPairs; - - class btQuantizedBvh* m_optimizedAabbTree; - - - bool m_ownsPairCache; - - btOverlapFilterCallback* m_filterCallback; - - int m_invalidPair; - - struct btBridgeProxy - { - btBroadphaseProxy* m_childProxy; - btBroadphaseInterface* m_childBroadphase; - }; - - -public: - - struct btMultiSapProxy : public btBroadphaseProxy - { - - ///array with all the entries that this proxy belongs to - btAlignedObjectArray m_bridgeProxies; - btVector3 m_aabbMin; - btVector3 m_aabbMax; - - int m_shapeType; - -/* void* m_userPtr; - short int m_collisionFilterGroup; - short int m_collisionFilterMask; -*/ - btMultiSapProxy(const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) - :btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask), - m_aabbMin(aabbMin), - m_aabbMax(aabbMax), - m_shapeType(shapeType) - { - m_multiSapParentProxy =this; - } - - - }; - -protected: - - - btAlignedObjectArray m_multiSapProxies; - -public: - - btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0); - - - btSapBroadphaseArray& getBroadphaseArray() - { - return m_sapBroadphases; - } - - const btSapBroadphaseArray& getBroadphaseArray() const - { - return m_sapBroadphases; - } - - virtual ~btMultiSapBroadphase(); - - virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy); - virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher); - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0)); - - void addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase); - - ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb - virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - - bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - - virtual btOverlappingPairCache* getOverlappingPairCache() - { - return m_overlappingPairs; - } - virtual const btOverlappingPairCache* getOverlappingPairCache() const - { - return m_overlappingPairs; - } - - ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame - ///will add some transform later - virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const - { - aabbMin.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT); - aabbMax.setValue(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT); - } - - void buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax); - - virtual void printStats(); - - void quicksort (btBroadphasePairArray& a, int lo, int hi); - - ///reset broadphase internal structures, to ensure determinism/reproducability - virtual void resetPool(btDispatcher* dispatcher); - -}; - -#endif //BT_MULTI_SAP_BROADPHASE diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp deleted file mode 100644 index 3beaf8658018..000000000000 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.cpp +++ /dev/null @@ -1,170 +0,0 @@ -/* -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. -*/ - -//btShapeHull was implemented by John McCutchan. - - -#include "btShapeHull.h" -#include "LinearMath/btConvexHull.h" - -#define NUM_UNITSPHERE_POINTS 42 - -btShapeHull::btShapeHull (const btConvexShape* shape) -{ - m_shape = shape; - m_vertices.clear (); - m_indices.clear(); - m_numIndices = 0; -} - -btShapeHull::~btShapeHull () -{ - m_indices.clear(); - m_vertices.clear (); -} - -bool -btShapeHull::buildHull (btScalar /*margin*/) -{ - int numSampleDirections = NUM_UNITSPHERE_POINTS; - { - int numPDA = m_shape->getNumPreferredPenetrationDirections(); - if (numPDA) - { - for (int i=0;igetPreferredPenetrationDirection(i,norm); - getUnitSpherePoints()[numSampleDirections] = norm; - numSampleDirections++; - } - } - } - - btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - int i; - for (i = 0; i < numSampleDirections; i++) - { - supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints()[i]); - } - - HullDesc hd; - hd.mFlags = QF_TRIANGLES; - hd.mVcount = static_cast(numSampleDirections); - -#ifdef BT_USE_DOUBLE_PRECISION - hd.mVertices = &supportPoints[0]; - hd.mVertexStride = sizeof(btVector3); -#else - hd.mVertices = &supportPoints[0]; - hd.mVertexStride = sizeof (btVector3); -#endif - - HullLibrary hl; - HullResult hr; - if (hl.CreateConvexHull (hd, hr) == QE_FAIL) - { - return false; - } - - m_vertices.resize (static_cast(hr.mNumOutputVertices)); - - - for (i = 0; i < static_cast(hr.mNumOutputVertices); i++) - { - m_vertices[i] = hr.m_OutputVertices[i]; - } - m_numIndices = hr.mNumIndices; - m_indices.resize(static_cast(m_numIndices)); - for (i = 0; i < static_cast(m_numIndices); i++) - { - m_indices[i] = hr.m_Indices[i]; - } - - // free temporary hull result that we just copied - hl.ReleaseResult (hr); - - return true; -} - -int -btShapeHull::numTriangles () const -{ - return static_cast(m_numIndices / 3); -} - -int -btShapeHull::numVertices () const -{ - return m_vertices.size (); -} - -int -btShapeHull::numIndices () const -{ - return static_cast(m_numIndices); -} - - -btVector3* btShapeHull::getUnitSpherePoints() -{ - static btVector3 sUnitSpherePoints[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)) - }; - return sUnitSpherePoints; -} - diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp deleted file mode 100644 index 4d92e853d3fa..000000000000 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ /dev/null @@ -1,305 +0,0 @@ -/* -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 "btPersistentManifold.h" -#include "LinearMath/btTransform.h" - - -btScalar gContactBreakingThreshold = btScalar(0.02); -ContactDestroyedCallback gContactDestroyedCallback = 0; -ContactProcessedCallback gContactProcessedCallback = 0; -///gContactCalcArea3Points will approximate the convex hull area using 3 points -///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower -bool gContactCalcArea3Points = true; - - -btPersistentManifold::btPersistentManifold() -:btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), -m_body0(0), -m_body1(0), -m_cachedPoints (0), -m_index1a(0) -{ -} - - - - -#ifdef DEBUG_PERSISTENCY -#include -void btPersistentManifold::DebugPersistency() -{ - int i; - printf("DebugPersistency : numPoints %d\n",m_cachedPoints); - for (i=0;i1) - 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 - } - - -} - -static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3) -{ - // It calculates possible 3 area constructed from random 4 points and returns the biggest one. - - btVector3 a[3],b[3]; - a[0] = p0 - p1; - a[1] = p0 - p2; - a[2] = p0 - p3; - b[0] = p2 - p3; - b[1] = p1 - p3; - b[2] = p1 - p2; - - //todo: Following 3 cross production can be easily optimized by SIMD. - btVector3 tmp0 = a[0].cross(b[0]); - btVector3 tmp1 = a[1].cross(b[1]); - btVector3 tmp2 = a[2].cross(b[2]); - - return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2()); -} - -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 (gContactCalcArea3Points) - { - 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(); - } - } - else - { - if(maxPenetrationIndex != 0) { - res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); - } - - if(maxPenetrationIndex != 1) { - res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); - } - - if(maxPenetrationIndex != 2) { - res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA); - } - - if(maxPenetrationIndex != 3) { - res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA); - } - } - 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, bool isPredictive) -{ - if (!isPredictive) - { - 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,(void*)m_body0,(void*)m_body1); - } - } - } -#ifdef DEBUG_PERSISTENCY - DebugPersistency(); -#endif // -} - - - - - diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp deleted file mode 100644 index 12997d2e3741..000000000000 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ /dev/null @@ -1,375 +0,0 @@ -#include "btMultiBodyConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) - - - -btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral) - :m_bodyA(bodyA), - m_bodyB(bodyB), - m_linkA(linkA), - m_linkB(linkB), - m_numRows(numRows), - m_jacSizeA(0), - m_jacSizeBoth(0), - m_isUnilateral(isUnilateral), - m_numDofsFinalized(-1), - m_maxAppliedImpulse(100) -{ - -} - -void btMultiBodyConstraint::updateJacobianSizes() -{ - if(m_bodyA) - { - m_jacSizeA = (6 + m_bodyA->getNumDofs()); - } - - if(m_bodyB) - { - m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs(); - } - else - m_jacSizeBoth = m_jacSizeA; -} - -void btMultiBodyConstraint::allocateJacobiansMultiDof() -{ - updateJacobianSizes(); - - m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); - m_data.resize((2 + m_jacSizeBoth) * m_numRows); -} - -btMultiBodyConstraint::~btMultiBodyConstraint() -{ -} - -void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) -{ - for (int i = 0; i < ndof; ++i) - data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; -} - -btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - btScalar* jacOrgA, btScalar* jacOrgB, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar posError, - const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, - btScalar relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) -{ - - - solverConstraint.m_multiBodyA = m_bodyA; - solverConstraint.m_multiBodyB = m_bodyB; - solverConstraint.m_linkA = m_linkA; - solverConstraint.m_linkB = m_linkB; - - btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; - btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - - btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); - btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); - - btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; - btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; - - btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary - if (bodyA) - rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin(); - - if (multiBodyA) - { - if (solverConstraint.m_linkA<0) - { - rel_pos1 = posAworld - multiBodyA->getBasePos(); - } else - { - rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); - } - - const int ndofA = multiBodyA->getNumDofs() + 6; - - solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (solverConstraint.m_deltaVelAindex <0) - { - solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); - } else - { - btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); - } - - //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom - //resize.. - solverConstraint.m_jacAindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofA); - //copy/determine - if(jacOrgA) - { - for (int i=0;ifillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); - } - - //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) - //resize.. - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - //determine.. - multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); - - btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormalOnB; - } - else //if(rb0) - { - btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormalOnB; - } - - if (multiBodyB) - { - if (solverConstraint.m_linkB<0) - { - rel_pos2 = posBworld - multiBodyB->getBasePos(); - } else - { - rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); - } - - const int ndofB = multiBodyB->getNumDofs() + 6; - - solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) - { - solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); - } - - //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom - //resize.. - solverConstraint.m_jacBindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofB); - //copy/determine.. - if(jacOrgB) - { - for (int i=0;ifillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); - } - - //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) - //resize.. - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - //determine.. - multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); - - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormalOnB; - - } - else //if(rb1) - { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormalOnB; - } - { - - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* deltaVelA = 0; - btScalar* deltaVelB = 0; - int ndofA = 0; - //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i]) - if (multiBodyA) - { - ndofA = multiBodyA->getNumDofs() + 6; - jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i] ; - btScalar l = deltaVelA[i]; - denom0 += j*l; - } - } - else if(rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); - } - // - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumDofs() + 6; - jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i] ; - btScalar l = deltaVelB[i]; - denom1 += j*l; - } - - } - else if(rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); - } - - // - btScalar d = denom0+denom1; - if (d>SIMD_EPSILON) - { - solverConstraint.m_jacDiagABInv = relaxation/(d); - } - else - { - //disable the constraint row to handle singularity/redundant constraint - solverConstraint.m_jacDiagABInv = 0.f; - } - } - - - //compute rhs and remaining solverConstraint fields - btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = multiBodyA->getNumDofs() + 6; - btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } - else if(rb0) - { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); - } - if (multiBodyB) - { - ndofB = multiBodyB->getNumDofs() + 6; - btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } - else if(rb1) - { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); - } - - solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; - } - - - ///warm starting (or zero if disabled) - /* - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - */ - - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - - { - - btScalar positionalError = 0.f; - btScalar velocityError = desiredVelocity - rel_vel;// * damping; - - - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - - positionalError = -penetration * erp/infoGlobal.m_timeStep; - - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; - solverConstraint.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - - solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = lowerLimit; - solverConstraint.m_upperLimit = upperLimit; - } - - return rel_vel; - -} diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h deleted file mode 100644 index 898669f86e2c..000000000000 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ /dev/null @@ -1,785 +0,0 @@ -/* -Copyright (c) 2003-2009 Erwin Coumans http://bullet.googlecode.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef BT_SCALAR_H -#define BT_SCALAR_H -#if defined(_MSC_VER) && defined(__clang__) /* clang supplies it's own overloads already */ -#define BT_NO_SIMD_OPERATOR_OVERLOADS -#endif - -#ifdef BT_MANAGED_CODE -//Aligned data types not supported in managed code -#pragma unmanaged -#endif - - -#include -#include //size_t for MSVC 6.0 -#include - -/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ -#define BT_BULLET_VERSION 284 - -inline int btGetVersion() -{ - return BT_BULLET_VERSION; -} - -#if defined(DEBUG) || defined (_DEBUG) -#define BT_DEBUG -#endif - - -#ifdef _WIN32 - - #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) - - #define SIMD_FORCE_INLINE inline - #define ATTRIBUTE_ALIGNED16(a) a - #define ATTRIBUTE_ALIGNED64(a) a - #define ATTRIBUTE_ALIGNED128(a) a - #elif (_M_ARM) - #define SIMD_FORCE_INLINE __forceinline - #define ATTRIBUTE_ALIGNED16(a) __declspec() a - #define ATTRIBUTE_ALIGNED64(a) __declspec() a - #define ATTRIBUTE_ALIGNED128(a) __declspec () a - #else - //#define BT_HAS_ALIGNED_ALLOCATOR - #pragma warning(disable : 4324) // disable padding warning -// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning. -// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines -// #pragma warning(disable:4786) // Disable the "debug name too long" warning - - #define SIMD_FORCE_INLINE __forceinline - #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a - #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a - #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a - #ifdef _XBOX - #define BT_USE_VMX128 - - #include - #define BT_HAVE_NATIVE_FSEL - #define btFsel(a,b,c) __fsel((a),(b),(c)) - #else - -#if defined (_M_ARM) - //Do not turn SSE on for ARM (may want to turn on BT_USE_NEON however) -#elif (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION)) - #if _MSC_VER>1400 - #define BT_USE_SIMD_VECTOR3 - #endif - - #define BT_USE_SSE - #ifdef BT_USE_SSE - -#if (_MSC_FULL_VER >= 170050727)//Visual Studio 2012 can compile SSE4/FMA3 (but SSE4/FMA3 is not enabled by default) - //#define BT_ALLOW_SSE4 //disable this cause blender targets sse2 -#endif //(_MSC_FULL_VER >= 160040219) - - //BT_USE_SSE_IN_API is disabled under Windows by default, because - //it makes it harder to integrate Bullet into your application under Windows - //(structured embedding Bullet structs/classes need to be 16-byte aligned) - //with relatively little performance gain - //If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries - //you can manually enable this line or set it in the build system for a bit of performance gain (a few percent, dependent on usage) - //#define BT_USE_SSE_IN_API - #endif //BT_USE_SSE - #include -#endif - - #endif//_XBOX - - #endif //__MINGW32__ - -#ifdef BT_DEBUG - #if defined(_MSC_VER) && !defined(__clang__) - #include - #define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak(); }} - #else//_MSC_VER - #include - #define btAssert assert - #endif//_MSC_VER -#else - #define btAssert(x) -#endif - //btFullAssert is optional, slows down a lot - #define btFullAssert(x) - - #define btLikely(_c) _c - #define btUnlikely(_c) _c - -#else - -#if defined (__CELLOS_LV2__) - #define SIMD_FORCE_INLINE inline __attribute__((always_inline)) - #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #ifndef assert - #include - #endif -#ifdef BT_DEBUG -#ifdef __SPU__ -#include -#define printf spu_printf - #define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}} -#else - #define btAssert assert -#endif - -#else - #define btAssert(x) -#endif - //btFullAssert is optional, slows down a lot - #define btFullAssert(x) - - #define btLikely(_c) _c - #define btUnlikely(_c) _c - -#else - -#ifdef USE_LIBSPE2 - - #define SIMD_FORCE_INLINE __inline - #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #ifndef assert - #include - #endif -#ifdef BT_DEBUG - #define btAssert assert -#else - #define btAssert(x) -#endif - //btFullAssert is optional, slows down a lot - #define btFullAssert(x) - - - #define btLikely(_c) __builtin_expect((_c), 1) - #define btUnlikely(_c) __builtin_expect((_c), 0) - - -#else - //non-windows systems - -#if (defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION))) - #if defined (__i386__) || defined (__x86_64__) - #define BT_USE_SIMD_VECTOR3 - #define BT_USE_SSE - //BT_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries - //if apps run into issues, we will disable the next line - #define BT_USE_SSE_IN_API - #ifdef BT_USE_SSE - // include appropriate SSE level - #if defined (__SSE4_1__) - #include - #elif defined (__SSSE3__) - #include - #elif defined (__SSE3__) - #include - #else - #include - #endif - #endif //BT_USE_SSE - #elif defined( __ARM_NEON__ ) - #ifdef __clang__ - #define BT_USE_NEON 1 - #define BT_USE_SIMD_VECTOR3 - - #if defined BT_USE_NEON && defined (__clang__) - #include - #endif//BT_USE_NEON - #endif //__clang__ - #endif//__arm__ - - #define SIMD_FORCE_INLINE inline __attribute__ ((always_inline)) -///@todo: check out alignment methods for other platforms/compilers - #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #ifndef assert - #include - #endif - - #if defined(DEBUG) || defined (_DEBUG) - #if defined (__i386__) || defined (__x86_64__) - #include - #define btAssert(x)\ - {\ - if(!(x))\ - {\ - printf("Assert %s in line %d, file %s\n",#x, __LINE__, __FILE__);\ - asm volatile ("int3");\ - }\ - } - #else//defined (__i386__) || defined (__x86_64__) - #define btAssert assert - #endif//defined (__i386__) || defined (__x86_64__) - #else//defined(DEBUG) || defined (_DEBUG) - #define btAssert(x) - #endif//defined(DEBUG) || defined (_DEBUG) - - //btFullAssert is optional, slows down a lot - #define btFullAssert(x) - #define btLikely(_c) _c - #define btUnlikely(_c) _c - -#else - - #define SIMD_FORCE_INLINE inline - ///@todo: check out alignment methods for other platforms/compilers - ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) - ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) - ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) - #define ATTRIBUTE_ALIGNED16(a) a - #define ATTRIBUTE_ALIGNED64(a) a - #define ATTRIBUTE_ALIGNED128(a) a - #ifndef assert - #include - #endif - -#if defined(DEBUG) || defined (_DEBUG) - #define btAssert assert -#else - #define btAssert(x) -#endif - - //btFullAssert is optional, slows down a lot - #define btFullAssert(x) - #define btLikely(_c) _c - #define btUnlikely(_c) _c -#endif //__APPLE__ - -#endif // LIBSPE2 - -#endif //__CELLOS_LV2__ -#endif - - -///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision. -#if defined(BT_USE_DOUBLE_PRECISION) - -typedef double btScalar; -//this number could be bigger in double precision -#define BT_LARGE_FLOAT 1e30 -#else - -typedef float btScalar; -//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX -#define BT_LARGE_FLOAT 1e18f -#endif - -#ifdef BT_USE_SSE -typedef __m128 btSimdFloat4; -#endif//BT_USE_SSE - -#if defined (BT_USE_SSE) -//#if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE) -#ifdef _WIN32 - -#ifndef BT_NAN -static int btNanMask = 0x7F800001; -#define BT_NAN (*(float*)&btNanMask) -#endif - -#ifndef BT_INFINITY -static int btInfinityMask = 0x7F800000; -#define BT_INFINITY (*(float*)&btInfinityMask) -inline int btGetInfinityMask()//suppress stupid compiler warning -{ - return btInfinityMask; -} -#endif - -//use this, in case there are clashes (such as xnamath.h) -#ifndef BT_NO_SIMD_OPERATOR_OVERLOADS -inline __m128 operator + (const __m128 A, const __m128 B) -{ - return _mm_add_ps(A, B); -} - -inline __m128 operator - (const __m128 A, const __m128 B) -{ - return _mm_sub_ps(A, B); -} - -inline __m128 operator * (const __m128 A, const __m128 B) -{ - return _mm_mul_ps(A, B); -} -#endif //BT_NO_SIMD_OPERATOR_OVERLOADS - -#define btCastfTo128i(a) (_mm_castps_si128(a)) -#define btCastfTo128d(a) (_mm_castps_pd(a)) -#define btCastiTo128f(a) (_mm_castsi128_ps(a)) -#define btCastdTo128f(a) (_mm_castpd_ps(a)) -#define btCastdTo128i(a) (_mm_castpd_si128(a)) -#define btAssign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3) - -#else//_WIN32 - -#define btCastfTo128i(a) ((__m128i)(a)) -#define btCastfTo128d(a) ((__m128d)(a)) -#define btCastiTo128f(a) ((__m128) (a)) -#define btCastdTo128f(a) ((__m128) (a)) -#define btCastdTo128i(a) ((__m128i)(a)) -#define btAssign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3} -#define BT_INFINITY INFINITY -#define BT_NAN NAN -#endif//_WIN32 -#else - -#ifdef BT_USE_NEON - #include - - typedef float32x4_t btSimdFloat4; - #define BT_INFINITY INFINITY - #define BT_NAN NAN - #define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3} -#else//BT_USE_NEON - - #ifndef BT_INFINITY - struct btInfMaskConverter - { - union { - float mask; - int intmask; - }; - btInfMaskConverter(int mask=0x7F800000) - :intmask(mask) - { - } - }; - static btInfMaskConverter btInfinityMask = 0x7F800000; - #define BT_INFINITY (btInfinityMask.mask) - inline int btGetInfinityMask()//suppress stupid compiler warning - { - return btInfinityMask.intmask; - } - #endif -#endif//BT_USE_NEON - -#endif //BT_USE_SSE - -#ifdef BT_USE_NEON -#include - -typedef float32x4_t btSimdFloat4; -#define BT_INFINITY INFINITY -#define BT_NAN NAN -#define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3} -#endif - - - - - -#define BT_DECLARE_ALIGNED_ALLOCATOR() \ - SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \ - SIMD_FORCE_INLINE void operator delete(void* ptr) { btAlignedFree(ptr); } \ - SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \ - SIMD_FORCE_INLINE void operator delete(void*, void*) { } \ - SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \ - SIMD_FORCE_INLINE void operator delete[](void* ptr) { btAlignedFree(ptr); } \ - SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \ - SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \ - - - -#if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS) - -SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); } -SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); } -SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); } -SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); } -SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); } -SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { if (xbtScalar(1)) x=btScalar(1); return acos(x); } -SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { if (xbtScalar(1)) x=btScalar(1); return asin(x); } -SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); } -SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); } -SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); } -SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); } -SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); } -SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); } - -#else - -SIMD_FORCE_INLINE btScalar btSqrt(btScalar y) -{ -#ifdef USE_APPROXIMATION -#ifdef __LP64__ - float xhalf = 0.5f*y; - int i = *(int*)&y; - i = 0x5f375a86 - (i>>1); - y = *(float*)&i; - y = y*(1.5f - xhalf*y*y); - y = y*(1.5f - xhalf*y*y); - y = y*(1.5f - xhalf*y*y); - y=1/y; - return y; -#else - double x, z, tempf; - unsigned long *tfptr = ((unsigned long *)&tempf) + 1; - tempf = y; - *tfptr = (0xbfcdd90a - *tfptr)>>1; /* estimate of 1/sqrt(y) */ - x = tempf; - z = y*btScalar(0.5); - x = (btScalar(1.5)*x)-(x*x)*(x*z); /* iteration formula */ - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - x = (btScalar(1.5)*x)-(x*x)*(x*z); - return x*y; -#endif -#else - return sqrtf(y); -#endif -} -SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); } -SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); } -SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); } -SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); } -SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { - if (xbtScalar(1)) - x=btScalar(1); - return acosf(x); -} -SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { - if (xbtScalar(1)) - x=btScalar(1); - return asinf(x); -} -SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } -SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } -SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); } -SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); } -SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); } -SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); } - -#endif - -#define SIMD_PI btScalar(3.1415926535897932384626433832795029) -#define SIMD_2_PI (btScalar(2.0) * SIMD_PI) -#define SIMD_HALF_PI (SIMD_PI * btScalar(0.5)) -#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0)) -#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI) -#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) - -#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */ -#define btRecip(x) (btScalar(1.0)/btScalar(x)) - -#ifdef BT_USE_DOUBLE_PRECISION -#define SIMD_EPSILON DBL_EPSILON -#define SIMD_INFINITY DBL_MAX -#define BT_ONE 1.0 -#define BT_ZERO 0.0 -#define BT_TWO 2.0 -#define BT_HALF 0.5 -#else -#define SIMD_EPSILON FLT_EPSILON -#define SIMD_INFINITY FLT_MAX -#define BT_ONE 1.0f -#define BT_ZERO 0.0f -#define BT_TWO 2.0f -#define BT_HALF 0.5f -#endif - -SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) -{ - btScalar coeff_1 = SIMD_PI / 4.0f; - btScalar coeff_2 = 3.0f * coeff_1; - btScalar abs_y = btFabs(y); - btScalar angle; - if (x >= 0.0f) { - btScalar r = (x - abs_y) / (x + abs_y); - angle = coeff_1 - coeff_1 * r; - } else { - btScalar r = (x + abs_y) / (abs_y - x); - angle = coeff_2 - coeff_1 * r; - } - return (y < 0.0f) ? -angle : angle; -} - -SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; } - -SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) { - return (((a) <= eps) && !((a) < -eps)); -} -SIMD_FORCE_INLINE bool btGreaterEqual (btScalar a, btScalar eps) { - return (!((a) <= eps)); -} - - -SIMD_FORCE_INLINE int btIsNegative(btScalar x) { - return x < btScalar(0.0) ? 1 : 0; -} - -SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; } -SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; } - -#define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name - -#ifndef btFsel -SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c) -{ - return a >= 0 ? b : c; -} -#endif -#define btFsels(a,b,c) (btScalar)btFsel(a,b,c) - - -SIMD_FORCE_INLINE bool btMachineIsLittleEndian() -{ - long int i = 1; - const char *p = (const char *) &i; - if (p[0] == 1) // Lowest address contains the least significant byte - return true; - else - return false; -} - - - -///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360 -///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html -SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) -{ - // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero - // Rely on positive value or'ed with its negative having sign bit on - // and zero value or'ed with its negative (which is still zero) having sign bit off - // Use arithmetic shift right, shifting the sign bit through all 32 bits - unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); - unsigned testEqz = ~testNz; - return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); -} -SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero) -{ - unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); - unsigned testEqz = ~testNz; - return static_cast((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); -} -SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) -{ -#ifdef BT_HAVE_NATIVE_FSEL - return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); -#else - return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; -#endif -} - -template SIMD_FORCE_INLINE void btSwap(T& a, T& b) -{ - T tmp = a; - a = b; - b = tmp; -} - - -//PCK: endian swapping functions -SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val) -{ - return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24)); -} - -SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val) -{ - return static_cast(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8)); -} - -SIMD_FORCE_INLINE unsigned btSwapEndian(int val) -{ - return btSwapEndian((unsigned)val); -} - -SIMD_FORCE_INLINE unsigned short btSwapEndian(short val) -{ - return btSwapEndian((unsigned short) val); -} - -///btSwapFloat uses using char pointers to swap the endianness -////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values -///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. -///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. -///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. -///so instead of returning a float/double, we return integer/long long integer -SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d) -{ - unsigned int a = 0; - unsigned char *dst = (unsigned char *)&a; - unsigned char *src = (unsigned char *)&d; - - dst[0] = src[3]; - dst[1] = src[2]; - dst[2] = src[1]; - dst[3] = src[0]; - return a; -} - -// unswap using char pointers -SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a) -{ - float d = 0.0f; - unsigned char *src = (unsigned char *)&a; - unsigned char *dst = (unsigned char *)&d; - - dst[0] = src[3]; - dst[1] = src[2]; - dst[2] = src[1]; - dst[3] = src[0]; - - return d; -} - - -// swap using char pointers -SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char* dst) -{ - unsigned char *src = (unsigned char *)&d; - - dst[0] = src[7]; - dst[1] = src[6]; - dst[2] = src[5]; - dst[3] = src[4]; - dst[4] = src[3]; - dst[5] = src[2]; - dst[6] = src[1]; - dst[7] = src[0]; - -} - -// unswap using char pointers -SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) -{ - double d = 0.0; - unsigned char *dst = (unsigned char *)&d; - - dst[0] = src[7]; - dst[1] = src[6]; - dst[2] = src[5]; - dst[3] = src[4]; - dst[4] = src[3]; - dst[5] = src[2]; - dst[6] = src[1]; - dst[7] = src[0]; - - return d; -} - -template -SIMD_FORCE_INLINE void btSetZero(T* a, int n) -{ - T* acurr = a; - size_t ncurr = n; - while (ncurr > 0) - { - *(acurr++) = 0; - --ncurr; - } -} - - -SIMD_FORCE_INLINE btScalar btLargeDot(const btScalar *a, const btScalar *b, int n) -{ - btScalar p0,q0,m0,p1,q1,m1,sum; - sum = 0; - n -= 2; - while (n >= 0) { - p0 = a[0]; q0 = b[0]; - m0 = p0 * q0; - p1 = a[1]; q1 = b[1]; - m1 = p1 * q1; - sum += m0; - sum += m1; - a += 2; - b += 2; - n -= 2; - } - n += 2; - while (n > 0) { - sum += (*a) * (*b); - a++; - b++; - n--; - } - return sum; -} - - -// returns normalized value in range [-SIMD_PI, SIMD_PI] -SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) -{ - angleInRadians = btFmod(angleInRadians, SIMD_2_PI); - if(angleInRadians < -SIMD_PI) - { - return angleInRadians + SIMD_2_PI; - } - else if(angleInRadians > SIMD_PI) - { - return angleInRadians - SIMD_2_PI; - } - else - { - return angleInRadians; - } -} - - - -///rudimentary class to provide type info -struct btTypedObject -{ - btTypedObject(int objectType) - :m_objectType(objectType) - { - } - int m_objectType; - inline int getObjectType() const - { - return m_objectType; - } -}; - - - -///align a pointer to the provided alignment, upwards -template T* btAlignPointer(T* unalignedPtr, size_t alignment) -{ - - struct btConvertPointerSizeT - { - union - { - T* ptr; - size_t integer; - }; - }; - btConvertPointerSizeT converter; - - - const size_t bit_mask = ~(alignment - 1); - converter.ptr = unalignedPtr; - converter.integer += alignment-1; - converter.integer &= bit_mask; - return converter.ptr; -} - - -#endif //BT_SCALAR_H diff --git a/extern/bullet2/src/LinearMath/btSerializer.cpp b/extern/bullet2/src/LinearMath/btSerializer.cpp deleted file mode 100644 index 8fdcfb142114..000000000000 --- a/extern/bullet2/src/LinearMath/btSerializer.cpp +++ /dev/null @@ -1,1177 +0,0 @@ -char sBulletDNAstr[]= { -char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(123),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), -char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95), -char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111), -char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), -char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(115),char(0),char(42),char(102),char(105),char(114),char(115),char(116),char(0),char(42),char(108),char(97),char(115), -char(116),char(0),char(109),char(95),char(102),char(108),char(111),char(97),char(116),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(108),char(91),char(51), -char(93),char(0),char(109),char(95),char(98),char(97),char(115),char(105),char(115),char(0),char(109),char(95),char(111),char(114),char(105),char(103),char(105),char(110),char(0),char(109), -char(95),char(114),char(111),char(111),char(116),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98), -char(116),char(114),char(101),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100), -char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(91),char(51),char(93),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122), -char(101),char(100),char(65),char(97),char(98),char(98),char(77),char(97),char(120),char(91),char(51),char(93),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77), -char(105),char(110),char(79),char(114),char(103),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77),char(97),char(120),char(79),char(114),char(103),char(0),char(109), -char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98),char(80),char(97), -char(114),char(116),char(0),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109), -char(95),char(112),char(97),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101), -char(120),char(79),char(114),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(98), -char(118),char(104),char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(0),char(109),char(95),char(98),char(118),char(104),char(65),char(97),char(98),char(98),char(77), -char(97),char(120),char(0),char(109),char(95),char(98),char(118),char(104),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110), -char(0),char(109),char(95),char(99),char(117),char(114),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(117),char(115), -char(101),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(110),char(117),char(109),char(67), -char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(76),char(101),char(97),char(102),char(78),char(111),char(100),char(101),char(115),char(0),char(109), -char(95),char(110),char(117),char(109),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117), -char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(99),char(111),char(110),char(116),char(105),char(103),char(117),char(111), -char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105), -char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116), -char(114),char(0),char(42),char(109),char(95),char(115),char(117),char(98),char(84),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(80),char(116),char(114),char(0), -char(109),char(95),char(116),char(114),char(97),char(118),char(101),char(114),char(115),char(97),char(108),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(110),char(117), -char(109),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(72),char(101),char(97),char(100),char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(110), -char(97),char(109),char(101),char(0),char(109),char(95),char(115),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(112),char(97), -char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110), -char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(83),char(99),char(97), -char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(101),char(78),char(111),char(114),char(109),char(97),char(108),char(0),char(109), -char(95),char(112),char(108),char(97),char(110),char(101),char(67),char(111),char(110),char(115),char(116),char(97),char(110),char(116),char(0),char(109),char(95),char(105),char(109),char(112), -char(108),char(105),char(99),char(105),char(116),char(83),char(104),char(97),char(112),char(101),char(68),char(105),char(109),char(101),char(110),char(115),char(105),char(111),char(110),char(115), -char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(109), -char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(111),char(115),char(0),char(109),char(95),char(114),char(97),char(100), -char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108), -char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(42),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(80),char(111), -char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(108),char(111),char(99), -char(97),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(83),char(105),char(122),char(101),char(0), -char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(0),char(109),char(95),char(112),char(97),char(100),char(91),char(50),char(93),char(0),char(109),char(95),char(118), -char(97),char(108),char(117),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(112),char(97),char(100),char(0),char(42),char(109),char(95),char(118),char(101), -char(114),char(116),char(105),char(99),char(101),char(115),char(51),char(102),char(0),char(42),char(109),char(95),char(118),char(101),char(114),char(116),char(105),char(99),char(101),char(115), -char(51),char(100),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(51),char(50),char(0),char(42),char(109),char(95),char(51), -char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(42),char(109),char(95),char(51),char(105),char(110),char(100),char(105),char(99),char(101), -char(115),char(56),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(109),char(95),char(110),char(117), -char(109),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(101),char(114),char(116), -char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(115),char(80),char(116),char(114), -char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(101),char(115),char(104), -char(80),char(97),char(114),char(116),char(115),char(0),char(109),char(95),char(109),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99), -char(101),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(70),char(108),char(111),char(97),char(116),char(66), -char(118),char(104),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(68),char(111),char(117),char(98),char(108), -char(101),char(66),char(118),char(104),char(0),char(42),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111), -char(77),char(97),char(112),char(0),char(109),char(95),char(112),char(97),char(100),char(51),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(114),char(105),char(109), -char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(116),char(114),char(97),char(110),char(115), -char(102),char(111),char(114),char(109),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(0),char(109), -char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104), -char(105),char(108),char(100),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104), -char(97),char(112),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(104),char(105),char(108),char(100),char(83),char(104),char(97), -char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(117),char(112),char(73),char(110),char(100), -char(101),char(120),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86), -char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103), -char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42), -char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101), -char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80), -char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95), -char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110), -char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114), -char(116),char(101),char(120),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(68), -char(105),char(115),char(116),char(97),char(110),char(99),char(101),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(122), -char(101),char(114),char(111),char(65),char(114),char(101),char(97),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110), -char(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83), -char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117), -char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(103),char(105),char(109),char(112),char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121), -char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115), -char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100), -char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117), -char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97), -char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0),char(42),char(109),char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104), -char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0),char(42),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105), -char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(42),char(109),char(95),char(114),char(111),char(111),char(116),char(67),char(111),char(108),char(108),char(105), -char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(109),char(95),char(119),char(111),char(114),char(108),char(100),char(84),char(114),char(97), -char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), -char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105), -char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101), -char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), -char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95), -char(97),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0), -char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(114),char(111),char(99),char(101),char(115),char(115),char(105),char(110),char(103),char(84), -char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116), -char(105),char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109), -char(95),char(114),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(114), -char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99), -char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(99),char(100),char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114), -char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(99),char(100),char(77),char(111),char(116),char(105),char(111),char(110),char(84), -char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116), -char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(111),char(108),char(108), -char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84), -char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95), -char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105), -char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67), -char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73), -char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(0),char(109),char(95),char(99),char(111),char(108),char(108), -char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110), -char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0), -char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97), -char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103), -char(117),char(108),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70), -char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101), -char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105), -char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0), -char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101), -char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112), -char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103), -char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103), -char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76), -char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), -char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103), -char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100), -char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117), -char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108), -char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111), -char(108),char(100),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103), -char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110), -char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65), -char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101), -char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(84),char(121),char(112), -char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(100), -char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97), -char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68), -char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108), -char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100), -char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109), -char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(98), -char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(104),char(114),char(101),char(115),char(104), -char(111),char(108),char(100),char(0),char(109),char(95),char(105),char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100), -char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97), -char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109), -char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101), -char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102), -char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), -char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108), -char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),char(114),char(84),char(97),char(114),char(103),char(101), -char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114), -char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116), -char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105), -char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116), -char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111), -char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119), -char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110), -char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(100),char(97),char(109),char(112), -char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109), -char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105), -char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105), -char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105), -char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110), -char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116), -char(70),char(111),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(114),char(97),char(109),char(101),char(0),char(109), -char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110), -char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105), -char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(83), -char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103), -char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(66), -char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82), -char(80),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95), -char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(108),char(105),char(110), -char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114), -char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(108),char(105),char(110), -char(101),char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(108), -char(105),char(110),char(101),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(108), -char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115), -char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105), -char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105), -char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(110),char(97),char(98), -char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83), -char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97), -char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(108), -char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115), -char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83), -char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91), -char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(66),char(111),char(117),char(110),char(99),char(101),char(0),char(109), -char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110), -char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), -char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114), -char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(84),char(97), -char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), -char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(97),char(110), -char(103),char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(97), -char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115), -char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109), -char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105), -char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114), -char(69),char(110),char(97),char(98),char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103), -char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95), -char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91), -char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116), -char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95), -char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103), -char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(111),char(116),char(97),char(116),char(101),char(79), -char(114),char(100),char(101),char(114),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),char(0),char(109),char(95),char(97),char(120),char(105), -char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),char(95),char(116),char(97),char(117),char(0),char(109), -char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),char(120),char(69),char(114),char(114),char(111),char(114), -char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),char(114),char(0),char(109),char(95),char(101),char(114), -char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),char(98),char(97),char(108),char(67),char(102),char(109), -char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(80),char(101),char(110),char(101),char(116), -char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(115),char(112), -char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),char(69),char(114),char(112),char(0),char(109),char(95), -char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),char(97),char(114),char(109),char(115),char(116),char(97), -char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(97),char(120),char(71),char(121),char(114), -char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(115),char(105),char(110),char(103),char(108), -char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110), -char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),char(109),char(73),char(116),char(101),char(114),char(97), -char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(77),char(111),char(100),char(101),char(0),char(109), -char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(82),char(101),char(115),char(116),char(105), -char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(109),char(105), -char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),char(99),char(104),char(83),char(105),char(122),char(101), -char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(105), -char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117), -char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109), -char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105), -char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(114),char(101),char(118), -char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99), -char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97),char(116),char(101),char(100),char(70),char(111),char(114), -char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95),char(97),char(114),char(101),char(97),char(0),char(109), -char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101), -char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110),char(103),char(116),char(104),char(0),char(109),char(95), -char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99), -char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114),char(101),char(97),char(0),char(109),char(95),char(99), -char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(52), -char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(99),char(49),char(0), -char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(70),char(114),char(97), -char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(0),char(109),char(95),char(110),char(111), -char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111),char(77),char(111),char(100),char(101),char(108),char(0), -char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95),char(100),char(114),char(97),char(103),char(0),char(109), -char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117),char(114),char(101),char(0),char(109),char(95),char(118), -char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105),char(99),char(70),char(114),char(105),char(99),char(116), -char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99),char(104),char(0),char(109),char(95),char(114),char(105), -char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109), -char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110), -char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114), -char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(72),char(97),char(114),char(100),char(110),char(101), -char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101), -char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101), -char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109), -char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100), -char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115), -char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111), -char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117), -char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67), -char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109), -char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(99),char(97), -char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73),char(116),char(101),char(114),char(97),char(116),char(105), -char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(73),char(116),char(101),char(114),char(97),char(116), -char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111), -char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111), -char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(97), -char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110), -char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(80),char(111), -char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87),char(101),char(105),char(103),char(116),char(115),char(0), -char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102),char(114),char(97),char(109),char(101),char(0),char(109), -char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(108),char(111),char(99),char(105),char(105),char(0), -char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91), -char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95), -char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(114),char(101),char(102),char(115), -char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109), -char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97),char(109),char(101),char(82),char(101),char(102),char(115), -char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(115), -char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(105),char(109),char(97),char(115),char(115), -char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(105),char(109), -char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95), -char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0), -char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(120),char(83),char(101),char(108),char(102), -char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(115), -char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(70), -char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105),char(110),char(115),char(65),char(110),char(99),char(104), -char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116), -char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(0),char(42),char(109),char(95), -char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(99),char(102), -char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101),char(108),char(101),char(116),char(101),char(0),char(109), -char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50),char(93),char(0),char(109),char(95),char(98),char(111), -char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(116),char(121),char(112),char(101),char(0), -char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(101),char(0), -char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100), -char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109),char(95),char(102),char(97),char(99),char(101),char(115), -char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(42),char(109),char(95),char(97),char(110), -char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(42),char(109), -char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(116),char(101),char(114),char(105),char(97), -char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70), -char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97), -char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(67), -char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74),char(111),char(105),char(110),char(116),char(115),char(0), -char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(109),char(95),char(122),char(101),char(114),char(111),char(82),char(111),char(116),char(80),char(97), -char(114),char(101),char(110),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(67), -char(111),char(109),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),char(102),char(115),char(101),char(116),char(0),char(109),char(95), -char(116),char(104),char(105),char(115),char(80),char(105),char(118),char(111),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102), -char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(84),char(111),char(112),char(91), -char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(66),char(111),char(116),char(116),char(111),char(109), -char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106), -char(111),char(105),char(110),char(116),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(67),char(111),char(108),char(108), -char(105),char(100),char(101),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109), -char(95),char(108),char(105),char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(73),char(110), -char(100),char(101),char(120),char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112),char(111),char(115), -char(86),char(97),char(114),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111),char(115),char(91), -char(55),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109),char(95),char(106), -char(111),char(105),char(110),char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(98),char(97),char(115), -char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101), -char(114),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111), -char(114),char(109),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98), -char(97),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(84),char(89),char(80),char(69),char(95),char(0),char(0),char(0),char(99),char(104),char(97),char(114), -char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0), -char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116), -char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114), -char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101), -char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51), -char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68), -char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105), -char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114), -char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116), -char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97), -char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101), -char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101), -char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97), -char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104), -char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105), -char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99), -char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110), -char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115), -char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101), -char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105), -char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110), -char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103), -char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114), -char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104), -char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104), -char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111), -char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110), -char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83), -char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104), -char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110), -char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104), -char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117), -char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115), -char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111), -char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114), -char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97), -char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108), -char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108), -char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82), -char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116), -char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), -char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116), -char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80), -char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110), -char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98), -char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), -char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108), -char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67), -char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111), -char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110), -char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115), -char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116), -char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110), -char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105), -char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114), -char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97), -char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111), -char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111), -char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111), -char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101), -char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108), -char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114), -char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66), -char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116), -char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100), -char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97), -char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97), -char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114), -char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68), -char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97), -char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(111),char(117),char(98), -char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105), -char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66), -char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116), -char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78), -char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0), -char(12),char(0),char(36),char(0),char(8),char(0),char(16),char(0),char(32),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0), -char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(84),char(0),char(-124),char(0),char(12),char(0),char(52),char(0),char(52),char(0), -char(20),char(0),char(64),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),char(32),char(0),char(28),char(0),char(60),char(0),char(56),char(0), -char(76),char(0),char(76),char(0),char(24),char(0),char(60),char(0),char(60),char(0),char(60),char(0),char(16),char(0),char(64),char(0),char(68),char(0),char(-48),char(1), -char(0),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-24),char(1),char(-96),char(3),char(8),char(0),char(52),char(0),char(52),char(0), -char(0),char(0),char(68),char(0),char(84),char(0),char(-124),char(0),char(116),char(0),char(92),char(1),char(-36),char(0),char(-116),char(1),char(124),char(1),char(-44),char(0), -char(-4),char(0),char(-52),char(1),char(92),char(1),char(116),char(2),char(-124),char(2),char(-76),char(4),char(-52),char(0),char(108),char(1),char(92),char(0),char(-116),char(0), -char(16),char(0),char(100),char(0),char(20),char(0),char(36),char(0),char(100),char(0),char(92),char(0),char(104),char(0),char(-64),char(0),char(92),char(1),char(104),char(0), -char(-84),char(1),char(-68),char(2),char(108),char(1),char(-68),char(0),char(100),char(0),char(0),char(0),char(83),char(84),char(82),char(67),char(84),char(0),char(0),char(0), -char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0), -char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0), -char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0), -char(15),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0), -char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0), -char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0), -char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0), -char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0), -char(0),char(0),char(21),char(0),char(23),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0), -char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0), -char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(25),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0), -char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0), -char(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0), -char(26),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0), -char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0), -char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(28),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0), -char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0), -char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0), -char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0), -char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0), -char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0), -char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0), -char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0), -char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0), -char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0), -char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0), -char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0), -char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0), -char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0), -char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0), -char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0), -char(46),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0), -char(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0), -char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0), -char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0), -char(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0), -char(4),char(0),char(96),char(0),char(48),char(0),char(5),char(0),char(29),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0), -char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(49),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0), -char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(104),char(0),char(20),char(0),char(105),char(0),char(14),char(0),char(106),char(0), -char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0), -char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0), -char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0), -char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(50),char(0),char(25),char(0),char(9),char(0),char(101),char(0), -char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(104),char(0),char(19),char(0),char(105),char(0), -char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0), -char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0), -char(7),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0), -char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(51),char(0),char(2),char(0), -char(52),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(53),char(0),char(2),char(0),char(54),char(0),char(124),char(0),char(13),char(0),char(125),char(0), -char(55),char(0),char(21),char(0),char(50),char(0),char(126),char(0),char(17),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0), -char(13),char(0),char(-126),char(0),char(13),char(0),char(-125),char(0),char(13),char(0),char(125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0), -char(13),char(0),char(-122),char(0),char(13),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0), -char(7),char(0),char(-117),char(0),char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0), -char(7),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(56),char(0),char(22),char(0),char(49),char(0),char(126),char(0),char(18),char(0),char(127),char(0), -char(14),char(0),char(-128),char(0),char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(125),char(0), -char(14),char(0),char(-124),char(0),char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0), -char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0), -char(8),char(0),char(-114),char(0),char(8),char(0),char(-113),char(0),char(8),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(0),char(0),char(37),char(0), -char(57),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-108),char(0), -char(55),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0), -char(4),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0), -char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(59),char(0),char(13),char(0),char(60),char(0),char(-108),char(0),char(60),char(0),char(-107),char(0), -char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0), -char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0), -char(4),char(0),char(-97),char(0),char(61),char(0),char(14),char(0),char(56),char(0),char(-108),char(0),char(56),char(0),char(-107),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0), -char(8),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0), -char(0),char(0),char(-96),char(0),char(62),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0), -char(63),char(0),char(3),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(64),char(0),char(3),char(0), -char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-95),char(0), -char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0), -char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(66),char(0),char(13),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0), -char(19),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0), -char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0), -char(7),char(0),char(-81),char(0),char(67),char(0),char(14),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0), -char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0), -char(8),char(0),char(-85),char(0),char(8),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0), -char(0),char(0),char(-80),char(0),char(68),char(0),char(10),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0), -char(8),char(0),char(-79),char(0),char(8),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0), -char(8),char(0),char(-81),char(0),char(8),char(0),char(-76),char(0),char(69),char(0),char(11),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0), -char(19),char(0),char(-91),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0), -char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0), -char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(71),char(0),char(9),char(0), -char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0), -char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(72),char(0),char(5),char(0), -char(70),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0), -char(73),char(0),char(5),char(0),char(71),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0), -char(8),char(0),char(-65),char(0),char(74),char(0),char(41),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0), -char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0),char(13),char(0),char(-62),char(0), -char(13),char(0),char(-61),char(0),char(13),char(0),char(-60),char(0),char(13),char(0),char(-59),char(0),char(13),char(0),char(-58),char(0),char(13),char(0),char(-57),char(0), -char(13),char(0),char(-56),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0), -char(0),char(0),char(-51),char(0),char(0),char(0),char(-50),char(0),char(0),char(0),char(-49),char(0),char(0),char(0),char(-80),char(0),char(13),char(0),char(-73),char(0), -char(13),char(0),char(-72),char(0),char(13),char(0),char(-48),char(0),char(13),char(0),char(-47),char(0),char(13),char(0),char(-46),char(0),char(13),char(0),char(-45),char(0), -char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0),char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0), -char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0), -char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-95),char(0), -char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),char(14),char(0),char(-64),char(0), -char(14),char(0),char(-63),char(0),char(14),char(0),char(-62),char(0),char(14),char(0),char(-61),char(0),char(14),char(0),char(-60),char(0),char(14),char(0),char(-59),char(0), -char(14),char(0),char(-58),char(0),char(14),char(0),char(-57),char(0),char(14),char(0),char(-56),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0), -char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0),char(0),char(0),char(-51),char(0),char(0),char(0),char(-50),char(0),char(0),char(0),char(-49),char(0), -char(0),char(0),char(-80),char(0),char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(14),char(0),char(-48),char(0),char(14),char(0),char(-47),char(0), -char(14),char(0),char(-46),char(0),char(14),char(0),char(-45),char(0),char(14),char(0),char(-44),char(0),char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0), -char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(14),char(0),char(-39),char(0),char(14),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0), -char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0), -char(76),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),char(7),char(0),char(-75),char(0), -char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0), -char(77),char(0),char(9),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(8),char(0),char(-75),char(0), -char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0), -char(78),char(0),char(5),char(0),char(58),char(0),char(-95),char(0),char(13),char(0),char(-31),char(0),char(13),char(0),char(-30),char(0),char(7),char(0),char(-29),char(0), -char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-31),char(0),char(14),char(0),char(-30),char(0), -char(8),char(0),char(-29),char(0),char(52),char(0),char(22),char(0),char(8),char(0),char(-28),char(0),char(8),char(0),char(-76),char(0),char(8),char(0),char(111),char(0), -char(8),char(0),char(-27),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(-26),char(0),char(8),char(0),char(-25),char(0),char(8),char(0),char(-24),char(0), -char(8),char(0),char(-23),char(0),char(8),char(0),char(-22),char(0),char(8),char(0),char(-21),char(0),char(8),char(0),char(-20),char(0),char(8),char(0),char(-19),char(0), -char(8),char(0),char(-18),char(0),char(8),char(0),char(-17),char(0),char(8),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0), -char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(22),char(0), -char(7),char(0),char(-28),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(113),char(0), -char(7),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),char(7),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0), -char(7),char(0),char(-21),char(0),char(7),char(0),char(-20),char(0),char(7),char(0),char(-19),char(0),char(7),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0), -char(7),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0), -char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(80),char(0),char(4),char(0),char(7),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0), -char(7),char(0),char(-8),char(0),char(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0),char(80),char(0),char(-7),char(0),char(13),char(0),char(-6),char(0), -char(13),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(7),char(0),char(-120),char(0), -char(7),char(0),char(-1),char(0),char(4),char(0),char(0),char(1),char(4),char(0),char(53),char(0),char(82),char(0),char(4),char(0),char(80),char(0),char(-7),char(0), -char(4),char(0),char(1),char(1),char(7),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(83),char(0),char(4),char(0),char(13),char(0),char(-2),char(0), -char(80),char(0),char(-7),char(0),char(4),char(0),char(4),char(1),char(7),char(0),char(5),char(1),char(84),char(0),char(7),char(0),char(13),char(0),char(6),char(1), -char(80),char(0),char(-7),char(0),char(4),char(0),char(7),char(1),char(7),char(0),char(8),char(1),char(7),char(0),char(9),char(1),char(7),char(0),char(10),char(1), -char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(11),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(12),char(1), -char(60),char(0),char(13),char(1),char(4),char(0),char(14),char(1),char(7),char(0),char(10),char(1),char(86),char(0),char(26),char(0),char(4),char(0),char(15),char(1), -char(7),char(0),char(16),char(1),char(7),char(0),char(-76),char(0),char(7),char(0),char(17),char(1),char(7),char(0),char(18),char(1),char(7),char(0),char(19),char(1), -char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),char(7),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1), -char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),char(7),char(0),char(27),char(1),char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1), -char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(7),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1), -char(4),char(0),char(35),char(1),char(4),char(0),char(36),char(1),char(4),char(0),char(37),char(1),char(4),char(0),char(38),char(1),char(4),char(0),char(118),char(0), -char(87),char(0),char(12),char(0),char(17),char(0),char(39),char(1),char(17),char(0),char(40),char(1),char(17),char(0),char(41),char(1),char(13),char(0),char(42),char(1), -char(13),char(0),char(43),char(1),char(7),char(0),char(44),char(1),char(4),char(0),char(45),char(1),char(4),char(0),char(46),char(1),char(4),char(0),char(47),char(1), -char(4),char(0),char(48),char(1),char(7),char(0),char(8),char(1),char(4),char(0),char(53),char(0),char(88),char(0),char(27),char(0),char(19),char(0),char(49),char(1), -char(17),char(0),char(50),char(1),char(17),char(0),char(51),char(1),char(13),char(0),char(42),char(1),char(13),char(0),char(52),char(1),char(13),char(0),char(53),char(1), -char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(7),char(0),char(58),char(1), -char(4),char(0),char(59),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(7),char(0),char(62),char(1),char(7),char(0),char(63),char(1), -char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(7),char(0),char(66),char(1),char(7),char(0),char(67),char(1),char(7),char(0),char(68),char(1), -char(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),char(4),char(0),char(72),char(1),char(4),char(0),char(73),char(1), -char(4),char(0),char(74),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(75),char(1),char(9),char(0),char(76),char(1),char(13),char(0),char(77),char(1), -char(7),char(0),char(78),char(1),char(7),char(0),char(-24),char(0),char(7),char(0),char(79),char(1),char(4),char(0),char(80),char(1),char(13),char(0),char(81),char(1), -char(4),char(0),char(82),char(1),char(4),char(0),char(83),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0), -char(50),char(0),char(126),char(0),char(87),char(0),char(85),char(1),char(80),char(0),char(86),char(1),char(81),char(0),char(87),char(1),char(82),char(0),char(88),char(1), -char(83),char(0),char(89),char(1),char(84),char(0),char(90),char(1),char(85),char(0),char(91),char(1),char(88),char(0),char(92),char(1),char(89),char(0),char(93),char(1), -char(4),char(0),char(94),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(95),char(1),char(4),char(0),char(96),char(1),char(4),char(0),char(97),char(1), -char(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),char(86),char(0),char(101),char(1),char(91),char(0),char(17),char(0), -char(16),char(0),char(102),char(1),char(14),char(0),char(103),char(1),char(14),char(0),char(104),char(1),char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1), -char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1),char(49),char(0),char(109),char(1),char(14),char(0),char(110),char(1),char(8),char(0),char(111),char(1), -char(4),char(0),char(112),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(113),char(1),char(4),char(0),char(114),char(1),char(8),char(0),char(115),char(1), -char(8),char(0),char(116),char(1),char(8),char(0),char(117),char(1),char(92),char(0),char(17),char(0),char(15),char(0),char(102),char(1),char(13),char(0),char(103),char(1), -char(13),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1),char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1), -char(50),char(0),char(109),char(1),char(13),char(0),char(110),char(1),char(4),char(0),char(113),char(1),char(7),char(0),char(111),char(1),char(4),char(0),char(112),char(1), -char(4),char(0),char(84),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1),char(7),char(0),char(117),char(1),char(4),char(0),char(114),char(1), -char(93),char(0),char(8),char(0),char(0),char(0),char(118),char(1),char(91),char(0),char(88),char(1),char(49),char(0),char(119),char(1),char(20),char(0),char(120),char(1), -char(14),char(0),char(121),char(1),char(4),char(0),char(95),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0), -char(0),char(0),char(118),char(1),char(92),char(0),char(88),char(1),char(50),char(0),char(119),char(1),char(19),char(0),char(120),char(1),char(13),char(0),char(121),char(1), -char(7),char(0),char(122),char(1),char(4),char(0),char(95),char(1),}; -int sBulletDNAlen= sizeof(sBulletDNAstr); - -char sBulletDNAstr64[]= { -char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(123),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109), -char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95), -char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(115),char(0),char(109),char(95),char(99),char(111), -char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(115),char(0),char(109),char(95),char(99),char(111),char(110), -char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(115),char(0),char(42),char(102),char(105),char(114),char(115),char(116),char(0),char(42),char(108),char(97),char(115), -char(116),char(0),char(109),char(95),char(102),char(108),char(111),char(97),char(116),char(115),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(108),char(91),char(51), -char(93),char(0),char(109),char(95),char(98),char(97),char(115),char(105),char(115),char(0),char(109),char(95),char(111),char(114),char(105),char(103),char(105),char(110),char(0),char(109), -char(95),char(114),char(111),char(111),char(116),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98), -char(116),char(114),char(101),char(101),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100), -char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(91),char(51),char(93),char(0),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122), -char(101),char(100),char(65),char(97),char(98),char(98),char(77),char(97),char(120),char(91),char(51),char(93),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77), -char(105),char(110),char(79),char(114),char(103),char(0),char(109),char(95),char(97),char(97),char(98),char(98),char(77),char(97),char(120),char(79),char(114),char(103),char(0),char(109), -char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(115),char(117),char(98),char(80),char(97), -char(114),char(116),char(0),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109), -char(95),char(112),char(97),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(101),char(115),char(99),char(97),char(112),char(101),char(73),char(110),char(100),char(101), -char(120),char(79),char(114),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(98), -char(118),char(104),char(65),char(97),char(98),char(98),char(77),char(105),char(110),char(0),char(109),char(95),char(98),char(118),char(104),char(65),char(97),char(98),char(98),char(77), -char(97),char(120),char(0),char(109),char(95),char(98),char(118),char(104),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110), -char(0),char(109),char(95),char(99),char(117),char(114),char(78),char(111),char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(117),char(115), -char(101),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(110),char(117),char(109),char(67), -char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(76),char(101),char(97),char(102),char(78),char(111),char(100),char(101),char(115),char(0),char(109), -char(95),char(110),char(117),char(109),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117), -char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(0),char(42),char(109),char(95),char(99),char(111),char(110),char(116),char(105),char(103),char(117),char(111), -char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105), -char(122),char(101),char(100),char(67),char(111),char(110),char(116),char(105),char(103),char(117),char(111),char(117),char(115),char(78),char(111),char(100),char(101),char(115),char(80),char(116), -char(114),char(0),char(42),char(109),char(95),char(115),char(117),char(98),char(84),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(80),char(116),char(114),char(0), -char(109),char(95),char(116),char(114),char(97),char(118),char(101),char(114),char(115),char(97),char(108),char(77),char(111),char(100),char(101),char(0),char(109),char(95),char(110),char(117), -char(109),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(72),char(101),char(97),char(100),char(101),char(114),char(115),char(0),char(42),char(109),char(95),char(110), -char(97),char(109),char(101),char(0),char(109),char(95),char(115),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(112),char(97), -char(100),char(100),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110), -char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(83),char(99),char(97), -char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(108),char(97),char(110),char(101),char(78),char(111),char(114),char(109),char(97),char(108),char(0),char(109), -char(95),char(112),char(108),char(97),char(110),char(101),char(67),char(111),char(110),char(115),char(116),char(97),char(110),char(116),char(0),char(109),char(95),char(105),char(109),char(112), -char(108),char(105),char(99),char(105),char(116),char(83),char(104),char(97),char(112),char(101),char(68),char(105),char(109),char(101),char(110),char(115),char(105),char(111),char(110),char(115), -char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(109), -char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(112),char(111),char(115),char(0),char(109),char(95),char(114),char(97),char(100), -char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(111),char(110),char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108), -char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(42),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(80),char(111), -char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95),char(108),char(111),char(99), -char(97),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(114),char(114),char(97),char(121),char(83),char(105),char(122),char(101),char(0), -char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(0),char(109),char(95),char(112),char(97),char(100),char(91),char(50),char(93),char(0),char(109),char(95),char(118), -char(97),char(108),char(117),char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(112),char(97),char(100),char(0),char(42),char(109),char(95),char(118),char(101), -char(114),char(116),char(105),char(99),char(101),char(115),char(51),char(102),char(0),char(42),char(109),char(95),char(118),char(101),char(114),char(116),char(105),char(99),char(101),char(115), -char(51),char(100),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(51),char(50),char(0),char(42),char(109),char(95),char(51), -char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(42),char(109),char(95),char(51),char(105),char(110),char(100),char(105),char(99),char(101), -char(115),char(56),char(0),char(42),char(109),char(95),char(105),char(110),char(100),char(105),char(99),char(101),char(115),char(49),char(54),char(0),char(109),char(95),char(110),char(117), -char(109),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(101),char(114),char(116), -char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(115),char(80),char(116),char(114), -char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(101),char(115),char(104), -char(80),char(97),char(114),char(116),char(115),char(0),char(109),char(95),char(109),char(101),char(115),char(104),char(73),char(110),char(116),char(101),char(114),char(102),char(97),char(99), -char(101),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(70),char(108),char(111),char(97),char(116),char(66), -char(118),char(104),char(0),char(42),char(109),char(95),char(113),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(68),char(111),char(117),char(98),char(108), -char(101),char(66),char(118),char(104),char(0),char(42),char(109),char(95),char(116),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111), -char(77),char(97),char(112),char(0),char(109),char(95),char(112),char(97),char(100),char(51),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(114),char(105),char(109), -char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(116),char(114),char(97),char(110),char(115), -char(102),char(111),char(114),char(109),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(0),char(109), -char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104),char(97),char(112),char(101),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104), -char(105),char(108),char(100),char(77),char(97),char(114),char(103),char(105),char(110),char(0),char(42),char(109),char(95),char(99),char(104),char(105),char(108),char(100),char(83),char(104), -char(97),char(112),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(104),char(105),char(108),char(100),char(83),char(104),char(97), -char(112),char(101),char(115),char(0),char(109),char(95),char(117),char(112),char(65),char(120),char(105),char(115),char(0),char(109),char(95),char(117),char(112),char(73),char(110),char(100), -char(101),char(120),char(0),char(109),char(95),char(102),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(48),char(86), -char(49),char(65),char(110),char(103),char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(49),char(86),char(50),char(65),char(110),char(103), -char(108),char(101),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(86),char(50),char(86),char(48),char(65),char(110),char(103),char(108),char(101),char(0),char(42), -char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(110),char(101), -char(120),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(118),char(97),char(108),char(117),char(101),char(65),char(114),char(114),char(97),char(121),char(80), -char(116),char(114),char(0),char(42),char(109),char(95),char(107),char(101),char(121),char(65),char(114),char(114),char(97),char(121),char(80),char(116),char(114),char(0),char(109),char(95), -char(99),char(111),char(110),char(118),char(101),char(120),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(112),char(108),char(97),char(110), -char(97),char(114),char(69),char(112),char(115),char(105),char(108),char(111),char(110),char(0),char(109),char(95),char(101),char(113),char(117),char(97),char(108),char(86),char(101),char(114), -char(116),char(101),char(120),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(101),char(100),char(103),char(101),char(68), -char(105),char(115),char(116),char(97),char(110),char(99),char(101),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(122), -char(101),char(114),char(111),char(65),char(114),char(101),char(97),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110), -char(101),char(120),char(116),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(104),char(97),char(115),char(104),char(84),char(97),char(98),char(108),char(101),char(83), -char(105),char(122),char(101),char(0),char(109),char(95),char(110),char(117),char(109),char(86),char(97),char(108),char(117),char(101),char(115),char(0),char(109),char(95),char(110),char(117), -char(109),char(75),char(101),char(121),char(115),char(0),char(109),char(95),char(103),char(105),char(109),char(112),char(97),char(99),char(116),char(83),char(117),char(98),char(84),char(121), -char(112),char(101),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115), -char(70),char(108),char(111),char(97),char(116),char(80),char(116),char(114),char(0),char(42),char(109),char(95),char(117),char(110),char(115),char(99),char(97),char(108),char(101),char(100), -char(80),char(111),char(105),char(110),char(116),char(115),char(68),char(111),char(117),char(98),char(108),char(101),char(80),char(116),char(114),char(0),char(109),char(95),char(110),char(117), -char(109),char(85),char(110),char(115),char(99),char(97),char(108),char(101),char(100),char(80),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(112),char(97), -char(100),char(100),char(105),char(110),char(103),char(51),char(91),char(52),char(93),char(0),char(42),char(109),char(95),char(98),char(114),char(111),char(97),char(100),char(112),char(104), -char(97),char(115),char(101),char(72),char(97),char(110),char(100),char(108),char(101),char(0),char(42),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(115),char(105), -char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(42),char(109),char(95),char(114),char(111),char(111),char(116),char(67),char(111),char(108),char(108),char(105), -char(115),char(105),char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(0),char(109),char(95),char(119),char(111),char(114),char(108),char(100),char(84),char(114),char(97), -char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), -char(111),char(110),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(105), -char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105),char(111),char(110),char(76),char(105),char(110),char(101),char(97),char(114),char(86),char(101), -char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(105),char(110),char(116),char(101),char(114),char(112),char(111),char(108),char(97),char(116),char(105), -char(111),char(110),char(65),char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95), -char(97),char(110),char(105),char(115),char(111),char(116),char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0), -char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(99),char(116),char(80),char(114),char(111),char(99),char(101),char(115),char(115),char(105),char(110),char(103),char(84), -char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(100),char(101),char(97),char(99),char(116),char(105),char(118),char(97),char(116), -char(105),char(111),char(110),char(84),char(105),char(109),char(101),char(0),char(109),char(95),char(102),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109), -char(95),char(114),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(114), -char(101),char(115),char(116),char(105),char(116),char(117),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(104),char(105),char(116),char(70),char(114),char(97),char(99), -char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(99),char(100),char(83),char(119),char(101),char(112),char(116),char(83),char(112),char(104),char(101),char(114), -char(101),char(82),char(97),char(100),char(105),char(117),char(115),char(0),char(109),char(95),char(99),char(99),char(100),char(77),char(111),char(116),char(105),char(111),char(110),char(84), -char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(104),char(97),char(115),char(65),char(110),char(105),char(115),char(111),char(116), -char(114),char(111),char(112),char(105),char(99),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(99),char(111),char(108),char(108), -char(105),char(115),char(105),char(111),char(110),char(70),char(108),char(97),char(103),char(115),char(0),char(109),char(95),char(105),char(115),char(108),char(97),char(110),char(100),char(84), -char(97),char(103),char(49),char(0),char(109),char(95),char(99),char(111),char(109),char(112),char(97),char(110),char(105),char(111),char(110),char(73),char(100),char(0),char(109),char(95), -char(97),char(99),char(116),char(105),char(118),char(97),char(116),char(105),char(111),char(110),char(83),char(116),char(97),char(116),char(101),char(49),char(0),char(109),char(95),char(105), -char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(84),char(121),char(112),char(101),char(0),char(109),char(95),char(99),char(104),char(101),char(99),char(107),char(67), -char(111),char(108),char(108),char(105),char(100),char(101),char(87),char(105),char(116),char(104),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(73), -char(110),char(102),char(111),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(0),char(109),char(95),char(99),char(111),char(108),char(108), -char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(105),char(110), -char(118),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(84),char(101),char(110),char(115),char(111),char(114),char(87),char(111),char(114),char(108),char(100),char(0), -char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97), -char(110),char(103),char(117),char(108),char(97),char(114),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103), -char(117),char(108),char(97),char(114),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(70), -char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(103),char(114),char(97),char(118),char(105),char(116),char(121),char(95),char(97),char(99),char(99),char(101), -char(108),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(105),char(110),char(118),char(73),char(110),char(101),char(114),char(116),char(105), -char(97),char(76),char(111),char(99),char(97),char(108),char(0),char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(70),char(111),char(114),char(99),char(101),char(0), -char(109),char(95),char(116),char(111),char(116),char(97),char(108),char(84),char(111),char(114),char(113),char(117),char(101),char(0),char(109),char(95),char(105),char(110),char(118),char(101), -char(114),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112), -char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103), -char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103), -char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(76), -char(105),char(110),char(101),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108), -char(100),char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103), -char(117),char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100), -char(83),char(113),char(114),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110),char(97),char(108),char(65),char(110),char(103),char(117), -char(108),char(97),char(114),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(108), -char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103),char(84),char(104),char(114),char(101),char(115),char(104),char(111), -char(108),char(100),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(108),char(101),char(101),char(112),char(105),char(110),char(103), -char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(97),char(100),char(100),char(105),char(116),char(105),char(111),char(110), -char(97),char(108),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(117),char(109),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(82),char(111),char(119),char(115),char(0),char(110),char(117),char(98),char(0),char(42),char(109),char(95),char(114),char(98),char(65), -char(0),char(42),char(109),char(95),char(114),char(98),char(66),char(0),char(109),char(95),char(111),char(98),char(106),char(101),char(99),char(116),char(84),char(121),char(112),char(101), -char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(84),char(121),char(112), -char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(100), -char(0),char(109),char(95),char(110),char(101),char(101),char(100),char(115),char(70),char(101),char(101),char(100),char(98),char(97),char(99),char(107),char(0),char(109),char(95),char(97), -char(112),char(112),char(108),char(105),char(101),char(100),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(100),char(98),char(103),char(68), -char(114),char(97),char(119),char(83),char(105),char(122),char(101),char(0),char(109),char(95),char(100),char(105),char(115),char(97),char(98),char(108),char(101),char(67),char(111),char(108), -char(108),char(105),char(115),char(105),char(111),char(110),char(115),char(66),char(101),char(116),char(119),char(101),char(101),char(110),char(76),char(105),char(110),char(107),char(101),char(100), -char(66),char(111),char(100),char(105),char(101),char(115),char(0),char(109),char(95),char(111),char(118),char(101),char(114),char(114),char(105),char(100),char(101),char(78),char(117),char(109), -char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(98), -char(114),char(101),char(97),char(107),char(105),char(110),char(103),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(104),char(114),char(101),char(115),char(104), -char(111),char(108),char(100),char(0),char(109),char(95),char(105),char(115),char(69),char(110),char(97),char(98),char(108),char(101),char(100),char(0),char(112),char(97),char(100),char(100), -char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(116),char(121),char(112),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97), -char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(65),char(0),char(109), -char(95),char(112),char(105),char(118),char(111),char(116),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(98),char(65),char(70),char(114),char(97),char(109),char(101), -char(0),char(109),char(95),char(114),char(98),char(66),char(70),char(114),char(97),char(109),char(101),char(0),char(109),char(95),char(117),char(115),char(101),char(82),char(101),char(102), -char(101),char(114),char(101),char(110),char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), -char(97),char(114),char(79),char(110),char(108),char(121),char(0),char(109),char(95),char(101),char(110),char(97),char(98),char(108),char(101),char(65),char(110),char(103),char(117),char(108), -char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(111),char(116),char(111),char(114),char(84),char(97),char(114),char(103),char(101), -char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(109),char(97),char(120),char(77),char(111),char(116),char(111),char(114), -char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105),char(116), -char(0),char(109),char(95),char(117),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(109),char(105), -char(116),char(83),char(111),char(102),char(116),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(98),char(105),char(97),char(115),char(70),char(97),char(99),char(116), -char(111),char(114),char(0),char(109),char(95),char(114),char(101),char(108),char(97),char(120),char(97),char(116),char(105),char(111),char(110),char(70),char(97),char(99),char(116),char(111), -char(114),char(0),char(109),char(95),char(112),char(97),char(100),char(100),char(105),char(110),char(103),char(49),char(91),char(52),char(93),char(0),char(109),char(95),char(115),char(119), -char(105),char(110),char(103),char(83),char(112),char(97),char(110),char(49),char(0),char(109),char(95),char(115),char(119),char(105),char(110),char(103),char(83),char(112),char(97),char(110), -char(50),char(0),char(109),char(95),char(116),char(119),char(105),char(115),char(116),char(83),char(112),char(97),char(110),char(0),char(109),char(95),char(100),char(97),char(109),char(112), -char(105),char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109), -char(105),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105), -char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(85),char(112),char(112),char(101),char(114),char(76),char(105),char(109),char(105), -char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(76),char(111),char(119),char(101),char(114),char(76),char(105),char(109),char(105), -char(116),char(0),char(109),char(95),char(117),char(115),char(101),char(76),char(105),char(110),char(101),char(97),char(114),char(82),char(101),char(102),char(101),char(114),char(101),char(110), -char(99),char(101),char(70),char(114),char(97),char(109),char(101),char(65),char(0),char(109),char(95),char(117),char(115),char(101),char(79),char(102),char(102),char(115),char(101),char(116), -char(70),char(111),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(114),char(97),char(109),char(101),char(0),char(109), -char(95),char(54),char(100),char(111),char(102),char(68),char(97),char(116),char(97),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(69),char(110), -char(97),char(98),char(108),char(101),char(100),char(91),char(54),char(93),char(0),char(109),char(95),char(101),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105), -char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103),char(83), -char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(91),char(54),char(93),char(0),char(109),char(95),char(115),char(112),char(114),char(105),char(110),char(103), -char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(91),char(54),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(66), -char(111),char(117),char(110),char(99),char(101),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82), -char(80),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95), -char(108),char(105),char(110),char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(108),char(105),char(110), -char(101),char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114), -char(84),char(97),char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(108),char(105),char(110), -char(101),char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(108), -char(105),char(110),char(101),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(108), -char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115), -char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105), -char(110),char(103),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105),char(98),char(114),char(105), -char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(69),char(110),char(97),char(98), -char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83), -char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97), -char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91),char(52),char(93),char(0),char(109),char(95),char(108), -char(105),char(110),char(101),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115), -char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(108),char(105),char(110),char(101),char(97),char(114),char(83), -char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91), -char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(66),char(111),char(117),char(110),char(99),char(101),char(0),char(109), -char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110), -char(103),char(117),char(108),char(97),char(114),char(83),char(116),char(111),char(112),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), -char(97),char(114),char(77),char(111),char(116),char(111),char(114),char(69),char(82),char(80),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114), -char(77),char(111),char(116),char(111),char(114),char(67),char(70),char(77),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(84),char(97), -char(114),char(103),char(101),char(116),char(86),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108), -char(97),char(114),char(77),char(97),char(120),char(77),char(111),char(116),char(111),char(114),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(97),char(110), -char(103),char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(84),char(97),char(114),char(103),char(101),char(116),char(0),char(109),char(95),char(97), -char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115), -char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109), -char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(113),char(117),char(105),char(108),char(105), -char(98),char(114),char(105),char(117),char(109),char(80),char(111),char(105),char(110),char(116),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114), -char(69),char(110),char(97),char(98),char(108),char(101),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103), -char(117),char(108),char(97),char(114),char(83),char(101),char(114),char(118),char(111),char(77),char(111),char(116),char(111),char(114),char(91),char(52),char(93),char(0),char(109),char(95), -char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(69),char(110),char(97),char(98),char(108),char(101),char(83),char(112),char(114),char(105),char(110),char(103),char(91), -char(52),char(93),char(0),char(109),char(95),char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(83),char(116), -char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95), -char(97),char(110),char(103),char(117),char(108),char(97),char(114),char(83),char(112),char(114),char(105),char(110),char(103),char(68),char(97),char(109),char(112),char(105),char(110),char(103), -char(76),char(105),char(109),char(105),char(116),char(101),char(100),char(91),char(52),char(93),char(0),char(109),char(95),char(114),char(111),char(116),char(97),char(116),char(101),char(79), -char(114),char(100),char(101),char(114),char(0),char(109),char(95),char(97),char(120),char(105),char(115),char(73),char(110),char(65),char(0),char(109),char(95),char(97),char(120),char(105), -char(115),char(73),char(110),char(66),char(0),char(109),char(95),char(114),char(97),char(116),char(105),char(111),char(0),char(109),char(95),char(116),char(97),char(117),char(0),char(109), -char(95),char(116),char(105),char(109),char(101),char(83),char(116),char(101),char(112),char(0),char(109),char(95),char(109),char(97),char(120),char(69),char(114),char(114),char(111),char(114), -char(82),char(101),char(100),char(117),char(99),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(115),char(111),char(114),char(0),char(109),char(95),char(101),char(114), -char(112),char(0),char(109),char(95),char(101),char(114),char(112),char(50),char(0),char(109),char(95),char(103),char(108),char(111),char(98),char(97),char(108),char(67),char(102),char(109), -char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(80),char(101),char(110),char(101),char(116), -char(114),char(97),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(115),char(112), -char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(84),char(117),char(114),char(110),char(69),char(114),char(112),char(0),char(109),char(95), -char(108),char(105),char(110),char(101),char(97),char(114),char(83),char(108),char(111),char(112),char(0),char(109),char(95),char(119),char(97),char(114),char(109),char(115),char(116),char(97), -char(114),char(116),char(105),char(110),char(103),char(70),char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(109),char(97),char(120),char(71),char(121),char(114), -char(111),char(115),char(99),char(111),char(112),char(105),char(99),char(70),char(111),char(114),char(99),char(101),char(0),char(109),char(95),char(115),char(105),char(110),char(103),char(108), -char(101),char(65),char(120),char(105),char(115),char(82),char(111),char(108),char(108),char(105),char(110),char(103),char(70),char(114),char(105),char(99),char(116),char(105),char(111),char(110), -char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(110),char(117),char(109),char(73),char(116),char(101),char(114),char(97), -char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(115),char(111),char(108),char(118),char(101),char(114),char(77),char(111),char(100),char(101),char(0),char(109), -char(95),char(114),char(101),char(115),char(116),char(105),char(110),char(103),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(82),char(101),char(115),char(116),char(105), -char(116),char(117),char(116),char(105),char(111),char(110),char(84),char(104),char(114),char(101),char(115),char(104),char(111),char(108),char(100),char(0),char(109),char(95),char(109),char(105), -char(110),char(105),char(109),char(117),char(109),char(83),char(111),char(108),char(118),char(101),char(114),char(66),char(97),char(116),char(99),char(104),char(83),char(105),char(122),char(101), -char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(108),char(105), -char(110),char(101),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(103),char(117), -char(108),char(97),char(114),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(118),char(111),char(108),char(117),char(109), -char(101),char(83),char(116),char(105),char(102),char(102),char(110),char(101),char(115),char(115),char(0),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105), -char(97),char(108),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(114),char(101),char(118), -char(105),char(111),char(117),char(115),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99), -char(105),char(116),char(121),char(0),char(109),char(95),char(97),char(99),char(99),char(117),char(109),char(117),char(108),char(97),char(116),char(101),char(100),char(70),char(111),char(114), -char(99),char(101),char(0),char(109),char(95),char(110),char(111),char(114),char(109),char(97),char(108),char(0),char(109),char(95),char(97),char(114),char(101),char(97),char(0),char(109), -char(95),char(97),char(116),char(116),char(97),char(99),char(104),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101), -char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(76),char(101),char(110),char(103),char(116),char(104),char(0),char(109),char(95), -char(98),char(98),char(101),char(110),char(100),char(105),char(110),char(103),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99), -char(101),char(115),char(91),char(51),char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(65),char(114),char(101),char(97),char(0),char(109),char(95),char(99), -char(48),char(91),char(52),char(93),char(0),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(91),char(52), -char(93),char(0),char(109),char(95),char(114),char(101),char(115),char(116),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(99),char(49),char(0), -char(109),char(95),char(99),char(50),char(0),char(109),char(95),char(99),char(48),char(0),char(109),char(95),char(108),char(111),char(99),char(97),char(108),char(70),char(114),char(97), -char(109),char(101),char(0),char(42),char(109),char(95),char(114),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(0),char(109),char(95),char(110),char(111), -char(100),char(101),char(73),char(110),char(100),char(101),char(120),char(0),char(109),char(95),char(97),char(101),char(114),char(111),char(77),char(111),char(100),char(101),char(108),char(0), -char(109),char(95),char(98),char(97),char(117),char(109),char(103),char(97),char(114),char(116),char(101),char(0),char(109),char(95),char(100),char(114),char(97),char(103),char(0),char(109), -char(95),char(108),char(105),char(102),char(116),char(0),char(109),char(95),char(112),char(114),char(101),char(115),char(115),char(117),char(114),char(101),char(0),char(109),char(95),char(118), -char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(100),char(121),char(110),char(97),char(109),char(105),char(99),char(70),char(114),char(105),char(99),char(116), -char(105),char(111),char(110),char(0),char(109),char(95),char(112),char(111),char(115),char(101),char(77),char(97),char(116),char(99),char(104),char(0),char(109),char(95),char(114),char(105), -char(103),char(105),char(100),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109), -char(95),char(107),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114),char(100),char(110), -char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(72),char(97),char(114), -char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(97),char(110),char(99),char(104),char(111),char(114),char(72),char(97),char(114),char(100),char(110),char(101), -char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115),char(116),char(101), -char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(75),char(105),char(110),char(101), -char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100),char(110),char(101),char(115),char(115),char(0),char(109), -char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(72),char(97),char(114),char(100), -char(110),char(101),char(115),char(115),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(67),char(108),char(117),char(115), -char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111), -char(102),char(116),char(75),char(105),char(110),char(101),char(116),char(105),char(99),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117), -char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(115),char(111),char(102),char(116),char(83),char(111),char(102),char(116),char(67), -char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(83),char(112),char(108),char(105),char(116),char(0),char(109), -char(95),char(109),char(97),char(120),char(86),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(116),char(105),char(109),char(101),char(83),char(99),char(97), -char(108),char(101),char(0),char(109),char(95),char(118),char(101),char(108),char(111),char(99),char(105),char(116),char(121),char(73),char(116),char(101),char(114),char(97),char(116),char(105), -char(111),char(110),char(115),char(0),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(73),char(116),char(101),char(114),char(97),char(116), -char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(100),char(114),char(105),char(102),char(116),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111), -char(110),char(115),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(73),char(116),char(101),char(114),char(97),char(116),char(105),char(111), -char(110),char(115),char(0),char(109),char(95),char(114),char(111),char(116),char(0),char(109),char(95),char(115),char(99),char(97),char(108),char(101),char(0),char(109),char(95),char(97), -char(113),char(113),char(0),char(109),char(95),char(99),char(111),char(109),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(105),char(116),char(105),char(111),char(110), -char(115),char(0),char(42),char(109),char(95),char(119),char(101),char(105),char(103),char(104),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(80),char(111), -char(115),char(105),char(116),char(105),char(111),char(110),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(87),char(101),char(105),char(103),char(116),char(115),char(0), -char(109),char(95),char(98),char(118),char(111),char(108),char(117),char(109),char(101),char(0),char(109),char(95),char(98),char(102),char(114),char(97),char(109),char(101),char(0),char(109), -char(95),char(102),char(114),char(97),char(109),char(101),char(120),char(102),char(111),char(114),char(109),char(0),char(109),char(95),char(108),char(111),char(99),char(105),char(105),char(0), -char(109),char(95),char(105),char(110),char(118),char(119),char(105),char(0),char(109),char(95),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91), -char(50),char(93),char(0),char(109),char(95),char(100),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(91),char(50),char(93),char(0),char(109),char(95), -char(108),char(118),char(0),char(109),char(95),char(97),char(118),char(0),char(42),char(109),char(95),char(102),char(114),char(97),char(109),char(101),char(114),char(101),char(102),char(115), -char(0),char(42),char(109),char(95),char(110),char(111),char(100),char(101),char(73),char(110),char(100),char(105),char(99),char(101),char(115),char(0),char(42),char(109),char(95),char(109), -char(97),char(115),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70),char(114),char(97),char(109),char(101),char(82),char(101),char(102),char(115), -char(0),char(109),char(95),char(110),char(117),char(109),char(78),char(111),char(100),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(115), -char(115),char(101),char(115),char(0),char(109),char(95),char(105),char(100),char(109),char(97),char(115),char(115),char(0),char(109),char(95),char(105),char(109),char(97),char(115),char(115), -char(0),char(109),char(95),char(110),char(118),char(105),char(109),char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(105),char(109), -char(112),char(117),char(108),char(115),char(101),char(115),char(0),char(109),char(95),char(110),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95), -char(108),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0),char(109),char(95),char(97),char(100),char(97),char(109),char(112),char(105),char(110),char(103),char(0), -char(109),char(95),char(109),char(97),char(116),char(99),char(104),char(105),char(110),char(103),char(0),char(109),char(95),char(109),char(97),char(120),char(83),char(101),char(108),char(102), -char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(0),char(109),char(95),char(115), -char(101),char(108),char(102),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(73),char(109),char(112),char(117),char(108),char(115),char(101),char(70), -char(97),char(99),char(116),char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(110),char(116),char(97),char(105),char(110),char(115),char(65),char(110),char(99),char(104), -char(111),char(114),char(0),char(109),char(95),char(99),char(111),char(108),char(108),char(105),char(100),char(101),char(0),char(109),char(95),char(99),char(108),char(117),char(115),char(116), -char(101),char(114),char(73),char(110),char(100),char(101),char(120),char(0),char(42),char(109),char(95),char(98),char(111),char(100),char(121),char(65),char(0),char(42),char(109),char(95), -char(98),char(111),char(100),char(121),char(66),char(0),char(109),char(95),char(114),char(101),char(102),char(115),char(91),char(50),char(93),char(0),char(109),char(95),char(99),char(102), -char(109),char(0),char(109),char(95),char(115),char(112),char(108),char(105),char(116),char(0),char(109),char(95),char(100),char(101),char(108),char(101),char(116),char(101),char(0),char(109), -char(95),char(114),char(101),char(108),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(91),char(50),char(93),char(0),char(109),char(95),char(98),char(111), -char(100),char(121),char(65),char(116),char(121),char(112),char(101),char(0),char(109),char(95),char(98),char(111),char(100),char(121),char(66),char(116),char(121),char(112),char(101),char(0), -char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(84),char(121),char(112),char(101),char(0),char(42),char(109),char(95),char(112),char(111),char(115),char(101),char(0), -char(42),char(42),char(109),char(95),char(109),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(115),char(0),char(42),char(109),char(95),char(110),char(111),char(100), -char(101),char(115),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(115),char(0),char(42),char(109),char(95),char(102),char(97),char(99),char(101),char(115), -char(0),char(42),char(109),char(95),char(116),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97),char(0),char(42),char(109),char(95),char(97),char(110), -char(99),char(104),char(111),char(114),char(115),char(0),char(42),char(109),char(95),char(99),char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(42),char(109), -char(95),char(106),char(111),char(105),char(110),char(116),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(77),char(97),char(116),char(101),char(114),char(105),char(97), -char(108),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(76),char(105),char(110),char(107),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(70), -char(97),char(99),char(101),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(84),char(101),char(116),char(114),char(97),char(104),char(101),char(100),char(114),char(97), -char(0),char(109),char(95),char(110),char(117),char(109),char(65),char(110),char(99),char(104),char(111),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(67), -char(108),char(117),char(115),char(116),char(101),char(114),char(115),char(0),char(109),char(95),char(110),char(117),char(109),char(74),char(111),char(105),char(110),char(116),char(115),char(0), -char(109),char(95),char(99),char(111),char(110),char(102),char(105),char(103),char(0),char(109),char(95),char(122),char(101),char(114),char(111),char(82),char(111),char(116),char(80),char(97), -char(114),char(101),char(110),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(67), -char(111),char(109),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102),char(102),char(115),char(101),char(116),char(0),char(109),char(95), -char(116),char(104),char(105),char(115),char(80),char(105),char(118),char(111),char(116),char(84),char(111),char(84),char(104),char(105),char(115),char(67),char(111),char(109),char(79),char(102), -char(102),char(115),char(101),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(84),char(111),char(112),char(91), -char(54),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(65),char(120),char(105),char(115),char(66),char(111),char(116),char(116),char(111),char(109), -char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(106), -char(111),char(105),char(110),char(116),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(108),char(105),char(110),char(107),char(67),char(111),char(108),char(108), -char(105),char(100),char(101),char(114),char(0),char(109),char(95),char(108),char(105),char(110),char(107),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109), -char(95),char(108),char(105),char(110),char(107),char(77),char(97),char(115),char(115),char(0),char(109),char(95),char(112),char(97),char(114),char(101),char(110),char(116),char(73),char(110), -char(100),char(101),char(120),char(0),char(109),char(95),char(100),char(111),char(102),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(112),char(111),char(115), -char(86),char(97),char(114),char(67),char(111),char(117),char(110),char(116),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(80),char(111),char(115),char(91), -char(55),char(93),char(0),char(109),char(95),char(106),char(111),char(105),char(110),char(116),char(86),char(101),char(108),char(91),char(54),char(93),char(0),char(109),char(95),char(106), -char(111),char(105),char(110),char(116),char(84),char(111),char(114),char(113),char(117),char(101),char(91),char(54),char(93),char(0),char(42),char(109),char(95),char(98),char(97),char(115), -char(101),char(78),char(97),char(109),char(101),char(0),char(42),char(109),char(95),char(98),char(97),char(115),char(101),char(67),char(111),char(108),char(108),char(105),char(100),char(101), -char(114),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(87),char(111),char(114),char(108),char(100),char(84),char(114),char(97),char(110),char(115),char(102),char(111), -char(114),char(109),char(0),char(109),char(95),char(98),char(97),char(115),char(101),char(73),char(110),char(101),char(114),char(116),char(105),char(97),char(0),char(109),char(95),char(98), -char(97),char(115),char(101),char(77),char(97),char(115),char(115),char(0),char(84),char(89),char(80),char(69),char(95),char(0),char(0),char(0),char(99),char(104),char(97),char(114), -char(0),char(117),char(99),char(104),char(97),char(114),char(0),char(115),char(104),char(111),char(114),char(116),char(0),char(117),char(115),char(104),char(111),char(114),char(116),char(0), -char(105),char(110),char(116),char(0),char(108),char(111),char(110),char(103),char(0),char(117),char(108),char(111),char(110),char(103),char(0),char(102),char(108),char(111),char(97),char(116), -char(0),char(100),char(111),char(117),char(98),char(108),char(101),char(0),char(118),char(111),char(105),char(100),char(0),char(80),char(111),char(105),char(110),char(116),char(101),char(114), -char(65),char(114),char(114),char(97),char(121),char(0),char(98),char(116),char(80),char(104),char(121),char(115),char(105),char(99),char(115),char(83),char(121),char(115),char(116),char(101), -char(109),char(0),char(76),char(105),char(115),char(116),char(66),char(97),char(115),char(101),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51), -char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(86),char(101),char(99),char(116),char(111),char(114),char(51),char(68), -char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114),char(110),char(105), -char(111),char(110),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(116),char(101),char(114), -char(110),char(105),char(111),char(110),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97),char(116), -char(114),char(105),char(120),char(51),char(120),char(51),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(97), -char(116),char(114),char(105),char(120),char(51),char(120),char(51),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(84),char(114),char(97),char(110),char(115),char(102),char(111),char(114),char(109),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(66),char(118),char(104),char(83),char(117),char(98),char(116),char(114),char(101),char(101),char(73),char(110),char(102),char(111),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101), -char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(79),char(112),char(116),char(105),char(109),char(105),char(122),char(101), -char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(78),char(111),char(100),char(101),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104),char(70),char(108),char(111),char(97), -char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(81),char(117),char(97),char(110),char(116),char(105),char(122),char(101),char(100),char(66),char(118),char(104), -char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105), -char(111),char(110),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(97),char(116),char(105),char(99), -char(80),char(108),char(97),char(110),char(101),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110), -char(118),char(101),char(120),char(73),char(110),char(116),char(101),char(114),char(110),char(97),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(80),char(111),char(115),char(105),char(116),char(105),char(111),char(110),char(65),char(110),char(100),char(82),char(97),char(100),char(105),char(117),char(115), -char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(83),char(112),char(104),char(101),char(114),char(101),char(83),char(104),char(97),char(112),char(101),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(68),char(97),char(116),char(97),char(0),char(98), -char(116),char(83),char(104),char(111),char(114),char(116),char(73),char(110),char(116),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105),char(112),char(108),char(101), -char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(104),char(97),char(114),char(73),char(110),char(100),char(101),char(120),char(84),char(114),char(105), -char(112),char(108),char(101),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(101),char(115),char(104),char(80),char(97),char(114),char(116),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(116),char(114),char(105),char(100),char(105),char(110),char(103),char(77),char(101),char(115),char(104),char(73),char(110), -char(116),char(101),char(114),char(102),char(97),char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103), -char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114), -char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110),char(102),char(111),char(77),char(97),char(112),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(83),char(99),char(97),char(108),char(101),char(100),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(77),char(101),char(115),char(104),char(83),char(104), -char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111),char(117),char(110),char(100),char(83),char(104), -char(97),char(112),char(101),char(67),char(104),char(105),char(108),char(100),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(109),char(112),char(111), -char(117),char(110),char(100),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(121),char(108),char(105),char(110), -char(100),char(101),char(114),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(83), -char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(97),char(112),char(115),char(117),char(108),char(101),char(83),char(104), -char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(114),char(105),char(97),char(110),char(103),char(108),char(101),char(73),char(110), -char(102),char(111),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(73),char(109),char(112),char(97),char(99),char(116),char(77),char(101),char(115),char(104), -char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(118),char(101),char(120),char(72),char(117), -char(108),char(108),char(83),char(104),char(97),char(112),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115), -char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(67),char(111),char(108),char(108),char(105),char(115),char(105),char(111),char(110),char(79),char(98),char(106),char(101),char(99),char(116),char(70),char(108),char(111), -char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114), -char(108),char(100),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97), -char(99),char(116),char(83),char(111),char(108),char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(68),char(121),char(110),char(97),char(109),char(105),char(99),char(115),char(87),char(111),char(114),char(108),char(100),char(70),char(108), -char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(116),char(97),char(99),char(116),char(83),char(111),char(108), -char(118),char(101),char(114),char(73),char(110),char(102),char(111),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82), -char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116), -char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0), -char(98),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(73),char(110),char(102),char(111),char(49),char(0),char(98),char(116), -char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116), -char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), -char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(82),char(105),char(103),char(105),char(100),char(66),char(111),char(100),char(121),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(84),char(121),char(112),char(101),char(100),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116), -char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80), -char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108),char(111),char(97),char(116),char(68), -char(97),char(116),char(97),char(0),char(98),char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110), -char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98), -char(116),char(80),char(111),char(105),char(110),char(116),char(50),char(80),char(111),char(105),char(110),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105), -char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108), -char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(72),char(105),char(110),char(103),char(101),char(67),char(111),char(110),char(115),char(116), -char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(67), -char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111), -char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(67),char(111),char(110),char(101),char(84),char(119),char(105),char(115),char(116), -char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110), -char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97), -char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(67),char(111),char(110),char(115), -char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116), -char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110), -char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105), -char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110), -char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114), -char(105),char(99),char(54),char(68),char(111),char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97), -char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(110),char(101),char(114),char(105),char(99),char(54),char(68),char(111), -char(102),char(83),char(112),char(114),char(105),char(110),char(103),char(50),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111), -char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(50),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101),char(114),char(67),char(111), -char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(83),char(108),char(105),char(100),char(101), -char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116), -char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114),char(97),char(105),char(110),char(116),char(70),char(108), -char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(71),char(101),char(97),char(114),char(67),char(111),char(110),char(115),char(116),char(114), -char(97),char(105),char(110),char(116),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66), -char(111),char(100),char(121),char(77),char(97),char(116),char(101),char(114),char(105),char(97),char(108),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116), -char(66),char(111),char(100),char(121),char(78),char(111),char(100),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100), -char(121),char(76),char(105),char(110),char(107),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(97), -char(99),char(101),char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(84),char(101),char(116),char(114),char(97), -char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(82),char(105),char(103),char(105),char(100),char(65),char(110),char(99),char(104),char(111),char(114), -char(68),char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(111),char(110),char(102),char(105),char(103),char(68), -char(97),char(116),char(97),char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(80),char(111),char(115),char(101),char(68),char(97),char(116),char(97), -char(0),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(67),char(108),char(117),char(115),char(116),char(101),char(114),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(74),char(111),char(105),char(110),char(116),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(83),char(111),char(102),char(116),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97), -char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105),char(110),char(107),char(68),char(111),char(117),char(98), -char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66),char(111),char(100),char(121),char(76),char(105), -char(110),char(107),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116),char(105),char(66), -char(111),char(100),char(121),char(68),char(111),char(117),char(98),char(108),char(101),char(68),char(97),char(116),char(97),char(0),char(98),char(116),char(77),char(117),char(108),char(116), -char(105),char(66),char(111),char(100),char(121),char(70),char(108),char(111),char(97),char(116),char(68),char(97),char(116),char(97),char(0),char(0),char(84),char(76),char(69),char(78), -char(1),char(0),char(1),char(0),char(2),char(0),char(2),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(0),char(0), -char(16),char(0),char(48),char(0),char(16),char(0),char(16),char(0),char(32),char(0),char(16),char(0),char(32),char(0),char(48),char(0),char(96),char(0),char(64),char(0), -char(-128),char(0),char(20),char(0),char(48),char(0),char(80),char(0),char(16),char(0),char(96),char(0),char(-112),char(0),char(16),char(0),char(56),char(0),char(56),char(0), -char(20),char(0),char(72),char(0),char(4),char(0),char(4),char(0),char(8),char(0),char(4),char(0),char(56),char(0),char(32),char(0),char(80),char(0),char(72),char(0), -char(96),char(0),char(80),char(0),char(32),char(0),char(64),char(0),char(64),char(0),char(64),char(0),char(16),char(0),char(72),char(0),char(80),char(0),char(-32),char(1), -char(16),char(1),char(-72),char(0),char(-104),char(0),char(104),char(0),char(88),char(0),char(-8),char(1),char(-80),char(3),char(8),char(0),char(64),char(0),char(64),char(0), -char(0),char(0),char(80),char(0),char(96),char(0),char(-112),char(0),char(-128),char(0),char(104),char(1),char(-24),char(0),char(-104),char(1),char(-120),char(1),char(-32),char(0), -char(8),char(1),char(-40),char(1),char(104),char(1),char(-128),char(2),char(-112),char(2),char(-64),char(4),char(-40),char(0),char(120),char(1),char(104),char(0),char(-104),char(0), -char(16),char(0),char(104),char(0),char(24),char(0),char(40),char(0),char(104),char(0),char(96),char(0),char(104),char(0),char(-56),char(0),char(104),char(1),char(112),char(0), -char(-32),char(1),char(-56),char(2),char(120),char(1),char(-56),char(0),char(112),char(0),char(0),char(0),char(83),char(84),char(82),char(67),char(84),char(0),char(0),char(0), -char(10),char(0),char(3),char(0),char(4),char(0),char(0),char(0),char(4),char(0),char(1),char(0),char(9),char(0),char(2),char(0),char(11),char(0),char(3),char(0), -char(10),char(0),char(3),char(0),char(10),char(0),char(4),char(0),char(10),char(0),char(5),char(0),char(12),char(0),char(2),char(0),char(9),char(0),char(6),char(0), -char(9),char(0),char(7),char(0),char(13),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(14),char(0),char(1),char(0),char(8),char(0),char(8),char(0), -char(15),char(0),char(1),char(0),char(7),char(0),char(8),char(0),char(16),char(0),char(1),char(0),char(8),char(0),char(8),char(0),char(17),char(0),char(1),char(0), -char(13),char(0),char(9),char(0),char(18),char(0),char(1),char(0),char(14),char(0),char(9),char(0),char(19),char(0),char(2),char(0),char(17),char(0),char(10),char(0), -char(13),char(0),char(11),char(0),char(20),char(0),char(2),char(0),char(18),char(0),char(10),char(0),char(14),char(0),char(11),char(0),char(21),char(0),char(4),char(0), -char(4),char(0),char(12),char(0),char(4),char(0),char(13),char(0),char(2),char(0),char(14),char(0),char(2),char(0),char(15),char(0),char(22),char(0),char(6),char(0), -char(13),char(0),char(16),char(0),char(13),char(0),char(17),char(0),char(4),char(0),char(18),char(0),char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0), -char(0),char(0),char(21),char(0),char(23),char(0),char(6),char(0),char(14),char(0),char(16),char(0),char(14),char(0),char(17),char(0),char(4),char(0),char(18),char(0), -char(4),char(0),char(19),char(0),char(4),char(0),char(20),char(0),char(0),char(0),char(21),char(0),char(24),char(0),char(3),char(0),char(2),char(0),char(14),char(0), -char(2),char(0),char(15),char(0),char(4),char(0),char(22),char(0),char(25),char(0),char(12),char(0),char(13),char(0),char(23),char(0),char(13),char(0),char(24),char(0), -char(13),char(0),char(25),char(0),char(4),char(0),char(26),char(0),char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0), -char(22),char(0),char(30),char(0),char(24),char(0),char(31),char(0),char(21),char(0),char(32),char(0),char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0), -char(26),char(0),char(12),char(0),char(14),char(0),char(23),char(0),char(14),char(0),char(24),char(0),char(14),char(0),char(25),char(0),char(4),char(0),char(26),char(0), -char(4),char(0),char(27),char(0),char(4),char(0),char(28),char(0),char(4),char(0),char(29),char(0),char(23),char(0),char(30),char(0),char(24),char(0),char(31),char(0), -char(4),char(0),char(33),char(0),char(4),char(0),char(34),char(0),char(21),char(0),char(32),char(0),char(27),char(0),char(3),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(36),char(0),char(0),char(0),char(37),char(0),char(28),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(13),char(0),char(39),char(0), -char(13),char(0),char(40),char(0),char(7),char(0),char(41),char(0),char(0),char(0),char(21),char(0),char(29),char(0),char(5),char(0),char(27),char(0),char(38),char(0), -char(13),char(0),char(39),char(0),char(13),char(0),char(42),char(0),char(7),char(0),char(43),char(0),char(4),char(0),char(44),char(0),char(30),char(0),char(2),char(0), -char(13),char(0),char(45),char(0),char(7),char(0),char(46),char(0),char(31),char(0),char(4),char(0),char(29),char(0),char(47),char(0),char(30),char(0),char(48),char(0), -char(4),char(0),char(49),char(0),char(0),char(0),char(37),char(0),char(32),char(0),char(1),char(0),char(4),char(0),char(50),char(0),char(33),char(0),char(2),char(0), -char(2),char(0),char(50),char(0),char(0),char(0),char(51),char(0),char(34),char(0),char(2),char(0),char(2),char(0),char(52),char(0),char(0),char(0),char(51),char(0), -char(35),char(0),char(2),char(0),char(0),char(0),char(52),char(0),char(0),char(0),char(53),char(0),char(36),char(0),char(8),char(0),char(13),char(0),char(54),char(0), -char(14),char(0),char(55),char(0),char(32),char(0),char(56),char(0),char(34),char(0),char(57),char(0),char(35),char(0),char(58),char(0),char(33),char(0),char(59),char(0), -char(4),char(0),char(60),char(0),char(4),char(0),char(61),char(0),char(37),char(0),char(4),char(0),char(36),char(0),char(62),char(0),char(13),char(0),char(63),char(0), -char(4),char(0),char(64),char(0),char(0),char(0),char(37),char(0),char(38),char(0),char(7),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0), -char(25),char(0),char(66),char(0),char(26),char(0),char(67),char(0),char(39),char(0),char(68),char(0),char(7),char(0),char(43),char(0),char(0),char(0),char(69),char(0), -char(40),char(0),char(2),char(0),char(38),char(0),char(70),char(0),char(13),char(0),char(39),char(0),char(41),char(0),char(4),char(0),char(19),char(0),char(71),char(0), -char(27),char(0),char(72),char(0),char(4),char(0),char(73),char(0),char(7),char(0),char(74),char(0),char(42),char(0),char(4),char(0),char(27),char(0),char(38),char(0), -char(41),char(0),char(75),char(0),char(4),char(0),char(76),char(0),char(7),char(0),char(43),char(0),char(43),char(0),char(3),char(0),char(29),char(0),char(47),char(0), -char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0),char(44),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(78),char(0), -char(0),char(0),char(37),char(0),char(45),char(0),char(3),char(0),char(29),char(0),char(47),char(0),char(4),char(0),char(77),char(0),char(0),char(0),char(37),char(0), -char(46),char(0),char(4),char(0),char(4),char(0),char(79),char(0),char(7),char(0),char(80),char(0),char(7),char(0),char(81),char(0),char(7),char(0),char(82),char(0), -char(39),char(0),char(14),char(0),char(4),char(0),char(83),char(0),char(4),char(0),char(84),char(0),char(46),char(0),char(85),char(0),char(4),char(0),char(86),char(0), -char(7),char(0),char(87),char(0),char(7),char(0),char(88),char(0),char(7),char(0),char(89),char(0),char(7),char(0),char(90),char(0),char(7),char(0),char(91),char(0), -char(4),char(0),char(92),char(0),char(4),char(0),char(93),char(0),char(4),char(0),char(94),char(0),char(4),char(0),char(95),char(0),char(0),char(0),char(37),char(0), -char(47),char(0),char(5),char(0),char(27),char(0),char(38),char(0),char(37),char(0),char(65),char(0),char(13),char(0),char(39),char(0),char(7),char(0),char(43),char(0), -char(4),char(0),char(96),char(0),char(48),char(0),char(5),char(0),char(29),char(0),char(47),char(0),char(13),char(0),char(97),char(0),char(14),char(0),char(98),char(0), -char(4),char(0),char(99),char(0),char(0),char(0),char(100),char(0),char(49),char(0),char(25),char(0),char(9),char(0),char(101),char(0),char(9),char(0),char(102),char(0), -char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(20),char(0),char(104),char(0),char(20),char(0),char(105),char(0),char(14),char(0),char(106),char(0), -char(14),char(0),char(107),char(0),char(14),char(0),char(108),char(0),char(8),char(0),char(109),char(0),char(8),char(0),char(110),char(0),char(8),char(0),char(111),char(0), -char(8),char(0),char(112),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(114),char(0),char(8),char(0),char(115),char(0),char(8),char(0),char(116),char(0), -char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0),char(4),char(0),char(121),char(0), -char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(50),char(0),char(25),char(0),char(9),char(0),char(101),char(0), -char(9),char(0),char(102),char(0),char(27),char(0),char(103),char(0),char(0),char(0),char(35),char(0),char(19),char(0),char(104),char(0),char(19),char(0),char(105),char(0), -char(13),char(0),char(106),char(0),char(13),char(0),char(107),char(0),char(13),char(0),char(108),char(0),char(7),char(0),char(109),char(0),char(7),char(0),char(110),char(0), -char(7),char(0),char(111),char(0),char(7),char(0),char(112),char(0),char(7),char(0),char(113),char(0),char(7),char(0),char(114),char(0),char(7),char(0),char(115),char(0), -char(7),char(0),char(116),char(0),char(4),char(0),char(117),char(0),char(4),char(0),char(118),char(0),char(4),char(0),char(119),char(0),char(4),char(0),char(120),char(0), -char(4),char(0),char(121),char(0),char(4),char(0),char(122),char(0),char(4),char(0),char(123),char(0),char(0),char(0),char(37),char(0),char(51),char(0),char(2),char(0), -char(52),char(0),char(124),char(0),char(14),char(0),char(125),char(0),char(53),char(0),char(2),char(0),char(54),char(0),char(124),char(0),char(13),char(0),char(125),char(0), -char(55),char(0),char(21),char(0),char(50),char(0),char(126),char(0),char(17),char(0),char(127),char(0),char(13),char(0),char(-128),char(0),char(13),char(0),char(-127),char(0), -char(13),char(0),char(-126),char(0),char(13),char(0),char(-125),char(0),char(13),char(0),char(125),char(0),char(13),char(0),char(-124),char(0),char(13),char(0),char(-123),char(0), -char(13),char(0),char(-122),char(0),char(13),char(0),char(-121),char(0),char(7),char(0),char(-120),char(0),char(7),char(0),char(-119),char(0),char(7),char(0),char(-118),char(0), -char(7),char(0),char(-117),char(0),char(7),char(0),char(-116),char(0),char(7),char(0),char(-115),char(0),char(7),char(0),char(-114),char(0),char(7),char(0),char(-113),char(0), -char(7),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(56),char(0),char(22),char(0),char(49),char(0),char(126),char(0),char(18),char(0),char(127),char(0), -char(14),char(0),char(-128),char(0),char(14),char(0),char(-127),char(0),char(14),char(0),char(-126),char(0),char(14),char(0),char(-125),char(0),char(14),char(0),char(125),char(0), -char(14),char(0),char(-124),char(0),char(14),char(0),char(-123),char(0),char(14),char(0),char(-122),char(0),char(14),char(0),char(-121),char(0),char(8),char(0),char(-120),char(0), -char(8),char(0),char(-119),char(0),char(8),char(0),char(-118),char(0),char(8),char(0),char(-117),char(0),char(8),char(0),char(-116),char(0),char(8),char(0),char(-115),char(0), -char(8),char(0),char(-114),char(0),char(8),char(0),char(-113),char(0),char(8),char(0),char(-112),char(0),char(4),char(0),char(-111),char(0),char(0),char(0),char(37),char(0), -char(57),char(0),char(2),char(0),char(4),char(0),char(-110),char(0),char(4),char(0),char(-109),char(0),char(58),char(0),char(13),char(0),char(55),char(0),char(-108),char(0), -char(55),char(0),char(-107),char(0),char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0), -char(4),char(0),char(-103),char(0),char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0), -char(7),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0),char(59),char(0),char(13),char(0),char(60),char(0),char(-108),char(0),char(60),char(0),char(-107),char(0), -char(0),char(0),char(35),char(0),char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0), -char(7),char(0),char(-102),char(0),char(7),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(7),char(0),char(-98),char(0), -char(4),char(0),char(-97),char(0),char(61),char(0),char(14),char(0),char(56),char(0),char(-108),char(0),char(56),char(0),char(-107),char(0),char(0),char(0),char(35),char(0), -char(4),char(0),char(-106),char(0),char(4),char(0),char(-105),char(0),char(4),char(0),char(-104),char(0),char(4),char(0),char(-103),char(0),char(8),char(0),char(-102),char(0), -char(8),char(0),char(-101),char(0),char(4),char(0),char(-100),char(0),char(4),char(0),char(-99),char(0),char(8),char(0),char(-98),char(0),char(4),char(0),char(-97),char(0), -char(0),char(0),char(-96),char(0),char(62),char(0),char(3),char(0),char(59),char(0),char(-95),char(0),char(13),char(0),char(-94),char(0),char(13),char(0),char(-93),char(0), -char(63),char(0),char(3),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(64),char(0),char(3),char(0), -char(59),char(0),char(-95),char(0),char(14),char(0),char(-94),char(0),char(14),char(0),char(-93),char(0),char(65),char(0),char(13),char(0),char(59),char(0),char(-95),char(0), -char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0), -char(7),char(0),char(-87),char(0),char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(66),char(0),char(13),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0), -char(19),char(0),char(-91),char(0),char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(7),char(0),char(-87),char(0), -char(7),char(0),char(-86),char(0),char(7),char(0),char(-85),char(0),char(7),char(0),char(-84),char(0),char(7),char(0),char(-83),char(0),char(7),char(0),char(-82),char(0), -char(7),char(0),char(-81),char(0),char(67),char(0),char(14),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0), -char(4),char(0),char(-90),char(0),char(4),char(0),char(-89),char(0),char(4),char(0),char(-88),char(0),char(8),char(0),char(-87),char(0),char(8),char(0),char(-86),char(0), -char(8),char(0),char(-85),char(0),char(8),char(0),char(-84),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0),char(8),char(0),char(-81),char(0), -char(0),char(0),char(-80),char(0),char(68),char(0),char(10),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0), -char(8),char(0),char(-79),char(0),char(8),char(0),char(-78),char(0),char(8),char(0),char(-77),char(0),char(8),char(0),char(-83),char(0),char(8),char(0),char(-82),char(0), -char(8),char(0),char(-81),char(0),char(8),char(0),char(-76),char(0),char(69),char(0),char(11),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0), -char(19),char(0),char(-91),char(0),char(7),char(0),char(-79),char(0),char(7),char(0),char(-78),char(0),char(7),char(0),char(-77),char(0),char(7),char(0),char(-83),char(0), -char(7),char(0),char(-82),char(0),char(7),char(0),char(-81),char(0),char(7),char(0),char(-76),char(0),char(0),char(0),char(21),char(0),char(70),char(0),char(9),char(0), -char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0), -char(13),char(0),char(-73),char(0),char(13),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(71),char(0),char(9),char(0), -char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0), -char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0),char(72),char(0),char(5),char(0), -char(70),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(7),char(0),char(-67),char(0),char(7),char(0),char(-66),char(0),char(7),char(0),char(-65),char(0), -char(73),char(0),char(5),char(0),char(71),char(0),char(-69),char(0),char(4),char(0),char(-68),char(0),char(8),char(0),char(-67),char(0),char(8),char(0),char(-66),char(0), -char(8),char(0),char(-65),char(0),char(74),char(0),char(41),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0), -char(13),char(0),char(-75),char(0),char(13),char(0),char(-74),char(0),char(13),char(0),char(-64),char(0),char(13),char(0),char(-63),char(0),char(13),char(0),char(-62),char(0), -char(13),char(0),char(-61),char(0),char(13),char(0),char(-60),char(0),char(13),char(0),char(-59),char(0),char(13),char(0),char(-58),char(0),char(13),char(0),char(-57),char(0), -char(13),char(0),char(-56),char(0),char(13),char(0),char(-55),char(0),char(13),char(0),char(-54),char(0),char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0), -char(0),char(0),char(-51),char(0),char(0),char(0),char(-50),char(0),char(0),char(0),char(-49),char(0),char(0),char(0),char(-80),char(0),char(13),char(0),char(-73),char(0), -char(13),char(0),char(-72),char(0),char(13),char(0),char(-48),char(0),char(13),char(0),char(-47),char(0),char(13),char(0),char(-46),char(0),char(13),char(0),char(-45),char(0), -char(13),char(0),char(-44),char(0),char(13),char(0),char(-43),char(0),char(13),char(0),char(-42),char(0),char(13),char(0),char(-41),char(0),char(13),char(0),char(-40),char(0), -char(13),char(0),char(-39),char(0),char(13),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0),char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0), -char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0),char(75),char(0),char(41),char(0),char(61),char(0),char(-95),char(0), -char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(14),char(0),char(-75),char(0),char(14),char(0),char(-74),char(0),char(14),char(0),char(-64),char(0), -char(14),char(0),char(-63),char(0),char(14),char(0),char(-62),char(0),char(14),char(0),char(-61),char(0),char(14),char(0),char(-60),char(0),char(14),char(0),char(-59),char(0), -char(14),char(0),char(-58),char(0),char(14),char(0),char(-57),char(0),char(14),char(0),char(-56),char(0),char(14),char(0),char(-55),char(0),char(14),char(0),char(-54),char(0), -char(0),char(0),char(-53),char(0),char(0),char(0),char(-52),char(0),char(0),char(0),char(-51),char(0),char(0),char(0),char(-50),char(0),char(0),char(0),char(-49),char(0), -char(0),char(0),char(-80),char(0),char(14),char(0),char(-73),char(0),char(14),char(0),char(-72),char(0),char(14),char(0),char(-48),char(0),char(14),char(0),char(-47),char(0), -char(14),char(0),char(-46),char(0),char(14),char(0),char(-45),char(0),char(14),char(0),char(-44),char(0),char(14),char(0),char(-43),char(0),char(14),char(0),char(-42),char(0), -char(14),char(0),char(-41),char(0),char(14),char(0),char(-40),char(0),char(14),char(0),char(-39),char(0),char(14),char(0),char(-38),char(0),char(0),char(0),char(-37),char(0), -char(0),char(0),char(-36),char(0),char(0),char(0),char(-35),char(0),char(0),char(0),char(-34),char(0),char(0),char(0),char(-33),char(0),char(4),char(0),char(-32),char(0), -char(76),char(0),char(9),char(0),char(59),char(0),char(-95),char(0),char(19),char(0),char(-92),char(0),char(19),char(0),char(-91),char(0),char(7),char(0),char(-75),char(0), -char(7),char(0),char(-74),char(0),char(7),char(0),char(-73),char(0),char(7),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0), -char(77),char(0),char(9),char(0),char(61),char(0),char(-95),char(0),char(20),char(0),char(-92),char(0),char(20),char(0),char(-91),char(0),char(8),char(0),char(-75),char(0), -char(8),char(0),char(-74),char(0),char(8),char(0),char(-73),char(0),char(8),char(0),char(-72),char(0),char(4),char(0),char(-71),char(0),char(4),char(0),char(-70),char(0), -char(78),char(0),char(5),char(0),char(58),char(0),char(-95),char(0),char(13),char(0),char(-31),char(0),char(13),char(0),char(-30),char(0),char(7),char(0),char(-29),char(0), -char(0),char(0),char(37),char(0),char(79),char(0),char(4),char(0),char(61),char(0),char(-95),char(0),char(14),char(0),char(-31),char(0),char(14),char(0),char(-30),char(0), -char(8),char(0),char(-29),char(0),char(52),char(0),char(22),char(0),char(8),char(0),char(-28),char(0),char(8),char(0),char(-76),char(0),char(8),char(0),char(111),char(0), -char(8),char(0),char(-27),char(0),char(8),char(0),char(113),char(0),char(8),char(0),char(-26),char(0),char(8),char(0),char(-25),char(0),char(8),char(0),char(-24),char(0), -char(8),char(0),char(-23),char(0),char(8),char(0),char(-22),char(0),char(8),char(0),char(-21),char(0),char(8),char(0),char(-20),char(0),char(8),char(0),char(-19),char(0), -char(8),char(0),char(-18),char(0),char(8),char(0),char(-17),char(0),char(8),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0), -char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0),char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(54),char(0),char(22),char(0), -char(7),char(0),char(-28),char(0),char(7),char(0),char(-76),char(0),char(7),char(0),char(111),char(0),char(7),char(0),char(-27),char(0),char(7),char(0),char(113),char(0), -char(7),char(0),char(-26),char(0),char(7),char(0),char(-25),char(0),char(7),char(0),char(-24),char(0),char(7),char(0),char(-23),char(0),char(7),char(0),char(-22),char(0), -char(7),char(0),char(-21),char(0),char(7),char(0),char(-20),char(0),char(7),char(0),char(-19),char(0),char(7),char(0),char(-18),char(0),char(7),char(0),char(-17),char(0), -char(7),char(0),char(-16),char(0),char(4),char(0),char(-15),char(0),char(4),char(0),char(-14),char(0),char(4),char(0),char(-13),char(0),char(4),char(0),char(-12),char(0), -char(4),char(0),char(-11),char(0),char(0),char(0),char(37),char(0),char(80),char(0),char(4),char(0),char(7),char(0),char(-10),char(0),char(7),char(0),char(-9),char(0), -char(7),char(0),char(-8),char(0),char(4),char(0),char(79),char(0),char(81),char(0),char(10),char(0),char(80),char(0),char(-7),char(0),char(13),char(0),char(-6),char(0), -char(13),char(0),char(-5),char(0),char(13),char(0),char(-4),char(0),char(13),char(0),char(-3),char(0),char(13),char(0),char(-2),char(0),char(7),char(0),char(-120),char(0), -char(7),char(0),char(-1),char(0),char(4),char(0),char(0),char(1),char(4),char(0),char(53),char(0),char(82),char(0),char(4),char(0),char(80),char(0),char(-7),char(0), -char(4),char(0),char(1),char(1),char(7),char(0),char(2),char(1),char(4),char(0),char(3),char(1),char(83),char(0),char(4),char(0),char(13),char(0),char(-2),char(0), -char(80),char(0),char(-7),char(0),char(4),char(0),char(4),char(1),char(7),char(0),char(5),char(1),char(84),char(0),char(7),char(0),char(13),char(0),char(6),char(1), -char(80),char(0),char(-7),char(0),char(4),char(0),char(7),char(1),char(7),char(0),char(8),char(1),char(7),char(0),char(9),char(1),char(7),char(0),char(10),char(1), -char(4),char(0),char(53),char(0),char(85),char(0),char(6),char(0),char(17),char(0),char(11),char(1),char(13),char(0),char(9),char(1),char(13),char(0),char(12),char(1), -char(60),char(0),char(13),char(1),char(4),char(0),char(14),char(1),char(7),char(0),char(10),char(1),char(86),char(0),char(26),char(0),char(4),char(0),char(15),char(1), -char(7),char(0),char(16),char(1),char(7),char(0),char(-76),char(0),char(7),char(0),char(17),char(1),char(7),char(0),char(18),char(1),char(7),char(0),char(19),char(1), -char(7),char(0),char(20),char(1),char(7),char(0),char(21),char(1),char(7),char(0),char(22),char(1),char(7),char(0),char(23),char(1),char(7),char(0),char(24),char(1), -char(7),char(0),char(25),char(1),char(7),char(0),char(26),char(1),char(7),char(0),char(27),char(1),char(7),char(0),char(28),char(1),char(7),char(0),char(29),char(1), -char(7),char(0),char(30),char(1),char(7),char(0),char(31),char(1),char(7),char(0),char(32),char(1),char(7),char(0),char(33),char(1),char(7),char(0),char(34),char(1), -char(4),char(0),char(35),char(1),char(4),char(0),char(36),char(1),char(4),char(0),char(37),char(1),char(4),char(0),char(38),char(1),char(4),char(0),char(118),char(0), -char(87),char(0),char(12),char(0),char(17),char(0),char(39),char(1),char(17),char(0),char(40),char(1),char(17),char(0),char(41),char(1),char(13),char(0),char(42),char(1), -char(13),char(0),char(43),char(1),char(7),char(0),char(44),char(1),char(4),char(0),char(45),char(1),char(4),char(0),char(46),char(1),char(4),char(0),char(47),char(1), -char(4),char(0),char(48),char(1),char(7),char(0),char(8),char(1),char(4),char(0),char(53),char(0),char(88),char(0),char(27),char(0),char(19),char(0),char(49),char(1), -char(17),char(0),char(50),char(1),char(17),char(0),char(51),char(1),char(13),char(0),char(42),char(1),char(13),char(0),char(52),char(1),char(13),char(0),char(53),char(1), -char(13),char(0),char(54),char(1),char(13),char(0),char(55),char(1),char(13),char(0),char(56),char(1),char(4),char(0),char(57),char(1),char(7),char(0),char(58),char(1), -char(4),char(0),char(59),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(61),char(1),char(7),char(0),char(62),char(1),char(7),char(0),char(63),char(1), -char(4),char(0),char(64),char(1),char(4),char(0),char(65),char(1),char(7),char(0),char(66),char(1),char(7),char(0),char(67),char(1),char(7),char(0),char(68),char(1), -char(7),char(0),char(69),char(1),char(7),char(0),char(70),char(1),char(7),char(0),char(71),char(1),char(4),char(0),char(72),char(1),char(4),char(0),char(73),char(1), -char(4),char(0),char(74),char(1),char(89),char(0),char(12),char(0),char(9),char(0),char(75),char(1),char(9),char(0),char(76),char(1),char(13),char(0),char(77),char(1), -char(7),char(0),char(78),char(1),char(7),char(0),char(-24),char(0),char(7),char(0),char(79),char(1),char(4),char(0),char(80),char(1),char(13),char(0),char(81),char(1), -char(4),char(0),char(82),char(1),char(4),char(0),char(83),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(53),char(0),char(90),char(0),char(19),char(0), -char(50),char(0),char(126),char(0),char(87),char(0),char(85),char(1),char(80),char(0),char(86),char(1),char(81),char(0),char(87),char(1),char(82),char(0),char(88),char(1), -char(83),char(0),char(89),char(1),char(84),char(0),char(90),char(1),char(85),char(0),char(91),char(1),char(88),char(0),char(92),char(1),char(89),char(0),char(93),char(1), -char(4),char(0),char(94),char(1),char(4),char(0),char(60),char(1),char(4),char(0),char(95),char(1),char(4),char(0),char(96),char(1),char(4),char(0),char(97),char(1), -char(4),char(0),char(98),char(1),char(4),char(0),char(99),char(1),char(4),char(0),char(100),char(1),char(86),char(0),char(101),char(1),char(91),char(0),char(17),char(0), -char(16),char(0),char(102),char(1),char(14),char(0),char(103),char(1),char(14),char(0),char(104),char(1),char(14),char(0),char(105),char(1),char(14),char(0),char(106),char(1), -char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1),char(49),char(0),char(109),char(1),char(14),char(0),char(110),char(1),char(8),char(0),char(111),char(1), -char(4),char(0),char(112),char(1),char(4),char(0),char(84),char(1),char(4),char(0),char(113),char(1),char(4),char(0),char(114),char(1),char(8),char(0),char(115),char(1), -char(8),char(0),char(116),char(1),char(8),char(0),char(117),char(1),char(92),char(0),char(17),char(0),char(15),char(0),char(102),char(1),char(13),char(0),char(103),char(1), -char(13),char(0),char(104),char(1),char(13),char(0),char(105),char(1),char(13),char(0),char(106),char(1),char(0),char(0),char(107),char(1),char(0),char(0),char(108),char(1), -char(50),char(0),char(109),char(1),char(13),char(0),char(110),char(1),char(4),char(0),char(113),char(1),char(7),char(0),char(111),char(1),char(4),char(0),char(112),char(1), -char(4),char(0),char(84),char(1),char(7),char(0),char(115),char(1),char(7),char(0),char(116),char(1),char(7),char(0),char(117),char(1),char(4),char(0),char(114),char(1), -char(93),char(0),char(8),char(0),char(0),char(0),char(118),char(1),char(91),char(0),char(88),char(1),char(49),char(0),char(119),char(1),char(20),char(0),char(120),char(1), -char(14),char(0),char(121),char(1),char(4),char(0),char(95),char(1),char(8),char(0),char(122),char(1),char(0),char(0),char(37),char(0),char(94),char(0),char(7),char(0), -char(0),char(0),char(118),char(1),char(92),char(0),char(88),char(1),char(50),char(0),char(119),char(1),char(19),char(0),char(120),char(1),char(13),char(0),char(121),char(1), -char(7),char(0),char(122),char(1),char(4),char(0),char(95),char(1),}; -int sBulletDNAlen64= sizeof(sBulletDNAstr64); diff --git a/source/gameengine/Converter/BL_BlenderDataConversion.cpp b/source/gameengine/Converter/BL_BlenderDataConversion.cpp index aec2e6f8c51c..26a1f77a5341 100644 --- a/source/gameengine/Converter/BL_BlenderDataConversion.cpp +++ b/source/gameengine/Converter/BL_BlenderDataConversion.cpp @@ -55,6 +55,7 @@ #include "mathfu.h" #include "PHY_IPhysicsEnvironment.h" +#include "DummyPhysicsEnvironment.h" #ifdef WITH_BULLET # include "CcdPhysicsEnvironment.h" @@ -112,6 +113,8 @@ #include "BL_ConvertObjectInfo.h" #include "BL_ArmatureObject.h" +#include "LA_SystemCommandLine.h" + #include "CM_Message.h" #include "GPU_texture.h" @@ -727,32 +730,21 @@ RAS_Deformer *BL_ConvertDeformer(KX_GameObject *object, KX_Mesh *meshobj) return deformer; } -static void BL_CreateGraphicObjectNew(KX_GameObject *gameobj, KX_Scene *kxscene, bool isActive, e_PhysicsEngine physics_engine) +static void BL_CreateGraphicObjectNew(KX_GameObject *gameobj, KX_Scene *kxscene, bool isActive, PHY_IPhysicsEnvironment *phyEnv) { - switch (physics_engine) { #ifdef WITH_BULLET - case UseBullet: - { - CcdPhysicsEnvironment *env = (CcdPhysicsEnvironment *)kxscene->GetPhysicsEnvironment(); - BLI_assert(env); - PHY_IMotionState *motionstate = new KX_MotionState(gameobj->GetNode()); - CcdGraphicController *ctrl = new CcdGraphicController(env, motionstate); - gameobj->SetGraphicController(ctrl); - ctrl->SetNewClientInfo(&gameobj->GetClientInfo()); - if (isActive) { - // add first, this will create the proxy handle, only if the object is visible - if (gameobj->GetVisible()) { - env->AddCcdGraphicController(ctrl); - } - } - break; - } -#endif - default: - { - break; + CcdPhysicsEnvironment *env = static_cast(phyEnv); + PHY_IMotionState *motionstate = new KX_MotionState(gameobj->GetNode()); + CcdGraphicController *ctrl = new CcdGraphicController(env, motionstate); + gameobj->SetGraphicController(ctrl); + ctrl->SetNewClientInfo(&gameobj->GetClientInfo()); + if (isActive) { + // add first, this will create the proxy handle, only if the object is visible + if (gameobj->GetVisible()) { + env->AddCcdGraphicController(ctrl); } } +#endif } static void BL_CreatePhysicsObjectNew(KX_GameObject *gameobj, Object *blenderobject, KX_Mesh *meshobj, @@ -1293,7 +1285,6 @@ static void bl_ConvertBlenderObject_Single(BL_SceneConverter& converter, void BL_ConvertBlenderObjects(struct Main *maggie, KX_Scene *kxscene, KX_KetsjiEngine *ketsjiEngine, - e_PhysicsEngine physics_engine, RAS_Rasterizer *rendertools, RAS_ICanvas *canvas, BL_SceneConverter& converter, @@ -1330,6 +1321,33 @@ void BL_ConvertBlenderObjects(struct Main *maggie, * and both are on the active layer. */ EXP_ListValue *convertedlist = new EXP_ListValue(); + // Find out which physics engine + PHY_IPhysicsEnvironment *phyEnv = nullptr; + e_PhysicsEngine physicsEngine = UseBullet; + + switch (blenderscene->gm.physicsEngine) { +#ifdef WITH_BULLET + case WOPHY_BULLET: + { + SYS_SystemHandle syshandle = SYS_GetSystem(); + int visualizePhysics = SYS_GetCommandLineInt(syshandle, "show_physics", 0); + + phyEnv = CcdPhysicsEnvironment::Create(blenderscene, visualizePhysics); + physicsEngine = UseBullet; + break; + } +#endif + case WOPHY_NONE: + default: + { + // We should probably use some sort of factory here + phyEnv = new DummyPhysicsEnvironment(); + physicsEngine = UseNone; + break; + } + } + kxscene->SetPhysicsEnvironment(phyEnv); + // Get the frame settings of the canvas. // Get the aspect ratio of the canvas as designed by the user. @@ -1617,8 +1635,10 @@ void BL_ConvertBlenderObjects(struct Main *maggie, continue; } - bool isactive = objectlist->SearchValue(gameobj); - BL_CreateGraphicObjectNew(gameobj, kxscene, isactive, physics_engine); + if (physicsEngine == UseBullet) { + const bool isactive = objectlist->SearchValue(gameobj); + BL_CreateGraphicObjectNew(gameobj, kxscene, isactive, phyEnv); + } if (gameobj->GetOccluder()) { occlusion = true; } diff --git a/source/gameengine/Converter/BL_BlenderDataConversion.h b/source/gameengine/Converter/BL_BlenderDataConversion.h index e8076f9b2285..724771038464 100644 --- a/source/gameengine/Converter/BL_BlenderDataConversion.h +++ b/source/gameengine/Converter/BL_BlenderDataConversion.h @@ -61,7 +61,7 @@ void BL_ConvertDerivedMeshToArray(DerivedMesh *dm, Mesh *me, const std::vector *BL_Converter::GetInactiveSceneNames() void BL_Converter::ConvertScene(BL_SceneConverter& converter, bool libloading) { KX_Scene *scene = converter.GetScene(); - // Find out which physics engine - Scene *blenderscene = scene->GetBlenderScene(); - - PHY_IPhysicsEnvironment *phy_env = nullptr; - - e_PhysicsEngine physics_engine = UseBullet; - - // This doesn't really seem to do anything except cause potential issues - // when doing threaded conversion, so it's disabled for now. - // SG_SetActiveStage(SG_STAGE_CONVERTER); - - switch (blenderscene->gm.physicsEngine) { -#ifdef WITH_BULLET - case WOPHY_BULLET: - { - SYS_SystemHandle syshandle = SYS_GetSystem(); /*unused*/ - int visualizePhysics = SYS_GetCommandLineInt(syshandle, "show_physics", 0); - - phy_env = CcdPhysicsEnvironment::Create(blenderscene, visualizePhysics); - physics_engine = UseBullet; - break; - } -#endif - default: - case WOPHY_NONE: - { - // We should probably use some sort of factory here - phy_env = new DummyPhysicsEnvironment(); - physics_engine = UseNone; - break; - } - } - - scene->SetPhysicsEnvironment(phy_env); BL_ConvertBlenderObjects( m_maggie, scene, m_ketsjiEngine, - physics_engine, m_ketsjiEngine->GetRasterizer(), m_ketsjiEngine->GetCanvas(), converter, diff --git a/source/gameengine/Ketsji/KX_CharacterWrapper.cpp b/source/gameengine/Ketsji/KX_CharacterWrapper.cpp index a16259728c71..7f617da4de5d 100644 --- a/source/gameengine/Ketsji/KX_CharacterWrapper.cpp +++ b/source/gameengine/Ketsji/KX_CharacterWrapper.cpp @@ -91,20 +91,18 @@ PyObject *KX_CharacterWrapper::pyattr_get_gravity(EXP_PyObjectPlus *self_v, cons { KX_CharacterWrapper *self = static_cast(self_v); - return PyFloat_FromDouble(self->m_character->GetGravity()); + return PyObjectFrom(self->m_character->GetGravity()); } int KX_CharacterWrapper::pyattr_set_gravity(EXP_PyObjectPlus *self_v, const EXP_PYATTRIBUTE_DEF *attrdef, PyObject *value) { KX_CharacterWrapper *self = static_cast(self_v); - double param = PyFloat_AsDouble(value); - - if (param == -1) { - PyErr_SetString(PyExc_ValueError, "KX_CharacterWrapper.gravity: expected a float"); + mt::vec3 gravity; + if (!PyVecTo(value, gravity)) { return PY_SET_ATTR_FAIL; } - self->m_character->SetGravity((float)param); + self->m_character->SetGravity(gravity); return PY_SET_ATTR_SUCCESS; } diff --git a/source/gameengine/Ketsji/KX_RenderData.cpp b/source/gameengine/Ketsji/KX_RenderData.cpp new file mode 100644 index 000000000000..abd1408f590e --- /dev/null +++ b/source/gameengine/Ketsji/KX_RenderData.cpp @@ -0,0 +1,48 @@ +#include "KX_RenderData.h" +#include "KX_Camera.h" + +KX_CameraRenderData::KX_CameraRenderData(KX_Camera *rendercam, KX_Camera *cullingcam, const RAS_Rect& area, + const RAS_Rect& viewport, RAS_Rasterizer::StereoMode stereoMode, RAS_Rasterizer::StereoEye eye, unsigned short index) + :m_renderCamera(rendercam), + m_cullingCamera(cullingcam), + m_area(area), + m_viewport(viewport), + m_stereoMode(stereoMode), + m_eye(eye), + m_index(index) +{ + m_renderCamera->AddRef(); +} + +KX_CameraRenderData::KX_CameraRenderData(const KX_CameraRenderData& other) + :m_renderCamera(CM_AddRef(other.m_renderCamera)), + m_cullingCamera(other.m_cullingCamera), + m_area(other.m_area), + m_viewport(other.m_viewport), + m_stereoMode(other.m_stereoMode), + m_eye(other.m_eye), + m_index(other.m_index) +{ +} + +KX_CameraRenderData::~KX_CameraRenderData() +{ + m_renderCamera->Release(); +} + +KX_SceneRenderData::KX_SceneRenderData(KX_Scene *scene) + :m_scene(scene) +{ +} + +KX_FrameRenderData::KX_FrameRenderData(RAS_Rasterizer::OffScreenType ofsType, const std::vector& eyes) + :m_ofsType(ofsType), + m_eyes(eyes) +{ +} + +KX_RenderData::KX_RenderData(RAS_Rasterizer::StereoMode stereoMode, bool renderPerEye) + :m_stereoMode(stereoMode), + m_renderPerEye(renderPerEye) +{ +} diff --git a/source/gameengine/Ketsji/KX_RenderData.h b/source/gameengine/Ketsji/KX_RenderData.h new file mode 100644 index 000000000000..be3708551a6a --- /dev/null +++ b/source/gameengine/Ketsji/KX_RenderData.h @@ -0,0 +1,60 @@ +#ifndef __KX_RENDER_DATA_H__ +#define __KX_RENDER_DATA_H__ + +#include "RAS_Rasterizer.h" + +class KX_Scene; +class KX_Camera; + +/** \brief This file contains all the data describing the rendering proceeded in a frame. + * KX_RenderData is the main data which for each eye (in case of stereo) contains a frame + * and each of these frame contains the scenes data and cameras data. + */ + +struct KX_CameraRenderData +{ + KX_CameraRenderData(KX_Camera *rendercam, KX_Camera *cullingcam, const RAS_Rect& area, const RAS_Rect& viewport, + RAS_Rasterizer::StereoMode stereoMode, RAS_Rasterizer::StereoEye eye, unsigned short index); + KX_CameraRenderData(const KX_CameraRenderData& other); + ~KX_CameraRenderData(); + + /// Rendered camera, could be a temporary camera in case of stereo. + KX_Camera *m_renderCamera; + KX_Camera *m_cullingCamera; + RAS_Rect m_area; + RAS_Rect m_viewport; + RAS_Rasterizer::StereoMode m_stereoMode; + RAS_Rasterizer::StereoEye m_eye; + // Index of the camera in all the scene's cameras rendered. + unsigned short m_index; +}; + +struct KX_SceneRenderData +{ + KX_SceneRenderData(KX_Scene *scene); + + KX_Scene *m_scene; + // Use multiple list of cameras in case of per eye stereo. + std::vector m_cameraDataList[RAS_Rasterizer::RAS_STEREO_MAXEYE]; +}; + +/// Data used to render a frame. +struct KX_FrameRenderData +{ + KX_FrameRenderData(RAS_Rasterizer::OffScreenType ofsType, const std::vector& eyes); + + RAS_Rasterizer::OffScreenType m_ofsType; + std::vector m_eyes; +}; + +struct KX_RenderData +{ + KX_RenderData(RAS_Rasterizer::StereoMode stereoMode, bool renderPerEye); + + RAS_Rasterizer::StereoMode m_stereoMode; + bool m_renderPerEye; + std::vector m_sceneDataList; + std::vector m_frameDataList; +}; + +#endif // __KX_RENDER_DATA_H__ diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp index 311eeb66059a..43337b207c3a 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp +++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.cpp @@ -44,7 +44,7 @@ #include "LinearMath/btConvexHull.h" #include "BulletCollision/Gimpact/btGImpactShape.h" -#include "BulletSoftBody/btSoftRigidDynamicsWorld.h" +#include "BulletSoftBody/btSoftRigidDynamicsWorldMt.h" #include "BLI_utildefines.h" @@ -59,7 +59,7 @@ float gAngularSleepingTreshold; CcdCharacter::CcdCharacter(CcdPhysicsController *ctrl, btMotionState *motionState, btPairCachingGhostObject *ghost, btConvexShape *shape, float stepHeight) - :btKinematicCharacterController(ghost, shape, stepHeight, 2), + :btKinematicCharacterController(ghost, shape, stepHeight, btVector3(0.0f, 0.0f, 1.0f)), m_ctrl(ctrl), m_motionState(motionState), m_jumps(0), @@ -500,6 +500,7 @@ bool CcdPhysicsController::CreateSoftbody() if (m_cci.m_do_anisotropic) { m_object->setAnisotropicFriction(m_cci.m_anisotropicFriction); } + return true; } @@ -654,7 +655,7 @@ bool CcdPhysicsController::ReplaceControllerShape(btCollisionShape *newShape) btSoftBody *softBody = GetSoftBody(); if (softBody) { - btSoftRigidDynamicsWorld *world = m_cci.m_physicsEnv->GetDynamicsWorld(); + btSoftRigidDynamicsWorldMt *world = m_cci.m_physicsEnv->GetDynamicsWorld(); // remove the old softBody world->removeSoftBody(softBody); @@ -1013,7 +1014,7 @@ void CcdPhysicsController::RefreshCollisions() return; } - btSoftRigidDynamicsWorld *dw = m_cci.m_physicsEnv->GetDynamicsWorld(); + btDynamicsWorld *dw = m_cci.m_physicsEnv->GetDynamicsWorld(); btBroadphaseProxy *proxy = m_object->getBroadphaseHandle(); btDispatcher *dispatcher = dw->getDispatcher(); btOverlappingPairCache *pairCache = dw->getPairCache(); diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsController.h b/source/gameengine/Physics/Bullet/CcdPhysicsController.h index f6974e7306a7..033d30c2db46 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsController.h +++ b/source/gameengine/Physics/Bullet/CcdPhysicsController.h @@ -476,13 +476,13 @@ class CcdCharacter : public btKinematicCharacterController, public PHY_ICharacte { return onGround(); } - virtual float GetGravity() + virtual mt::vec3 GetGravity() { - return getGravity(); + return ToMt(getGravity()); } - virtual void SetGravity(float gravity) + virtual void SetGravity(const mt::vec3& gravity) { - setGravity(gravity); + setGravity(ToBullet(gravity)); } virtual unsigned char GetMaxJumps() { diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp index 29c854e7c672..5fcedefdd849 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp @@ -27,10 +27,12 @@ #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/CollisionDispatch/btGhostObject.h" #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include "BulletSoftBody/btSoftRigidDynamicsWorld.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h" +#include "BulletSoftBody/btSoftRigidDynamicsWorldMt.h" #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" #include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h" #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h" #include "BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h" #include "BulletDynamics/MLCPSolvers/btMLCPSolver.h" #include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" @@ -392,8 +394,14 @@ void CcdPhysicsEnvironment::SetDebugDrawer(btIDebugDraw *debugDrawer) CcdPhysicsEnvironment::CcdPhysicsEnvironment(PHY_SolverType solverType, bool useDbvtCulling) :m_debugDrawer(nullptr), + m_collisionConfiguration(new btSoftBodyRigidBodyCollisionConfiguration()), + m_broadphase(new btDbvtBroadphase()), m_cullingCache(nullptr), m_cullingTree(nullptr), + m_solverMt(new btSequentialImpulseConstraintSolverMt()), + m_filterCallback(new CcdOverlapFilterCallBack(this)), + m_ghostPairCallback(new btGhostPairCallback()), + m_dispatcher(new btCollisionDispatcherMt(m_collisionConfiguration.get())), m_numIterations(10), m_numTimeSubSteps(1), m_ccdMode(0), @@ -401,37 +409,36 @@ CcdPhysicsEnvironment::CcdPhysicsEnvironment(PHY_SolverType solverType, bool use m_deactivationTime(2.0f), m_linearDeactivationThreshold(0.8f), m_angularDeactivationThreshold(1.0f), - m_contactBreakingThreshold(0.02f), - m_solver(nullptr), - m_filterCallback(nullptr), - m_ghostPairCallback(nullptr), - m_ownDispatcher(nullptr) + m_contactBreakingThreshold(0.02f) { + // Initialize the task scheduler used for bullet parallelization. + btITaskScheduler *scheduler = btGetTBBTaskScheduler(); + const int numThread = scheduler->getMaxNumThreads(); + if (btGetTaskScheduler() != scheduler) { + scheduler->setNumThreads(numThread); + btSetTaskScheduler(scheduler); + } + for (int i = 0; i < PHY_NUM_RESPONSE; i++) { m_triggerCallbacks[i] = nullptr; } - m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); - - btCollisionDispatcher *dispatcher = new btCollisionDispatcher(m_collisionConfiguration); - btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher); - m_ownDispatcher = dispatcher; + btGImpactCollisionAlgorithm::registerAlgorithm(m_dispatcher.get()); - m_broadphase = new btDbvtBroadphase(); // avoid any collision in the culling tree if (useDbvtCulling) { - m_cullingCache = new btNullPairCache(); - m_cullingTree = new btDbvtBroadphase(m_cullingCache); + m_cullingCache.reset(new btNullPairCache()); + m_cullingTree.reset(new btDbvtBroadphase(m_cullingCache.get())); } - m_filterCallback = new CcdOverlapFilterCallBack(this); - m_ghostPairCallback = new btGhostPairCallback(); - m_broadphase->getOverlappingPairCache()->setOverlapFilterCallback(m_filterCallback); - m_broadphase->getOverlappingPairCache()->setInternalGhostPairCallback(m_ghostPairCallback); + m_broadphase->getOverlappingPairCache()->setOverlapFilterCallback(m_filterCallback.get()); + m_broadphase->getOverlappingPairCache()->setInternalGhostPairCallback(m_ghostPairCallback.get()); + m_solvers.resize(numThread); SetSolverType(solverType); - - m_dynamicsWorld = new btSoftRigidDynamicsWorld(dispatcher, m_broadphase, m_solver, m_collisionConfiguration); + + m_solverPool.reset(new btConstraintSolverPoolMt(m_solvers.data(), numThread)); + m_dynamicsWorld.reset(new btSoftRigidDynamicsWorldMt(m_dispatcher.get(), m_broadphase.get(), m_solverPool.get(), m_solverMt.get(), m_collisionConfiguration.get())); m_dynamicsWorld->setInternalTickCallback(&CcdPhysicsEnvironment::StaticSimulationSubtickCallback, this); SetGravity(0.0f, 0.0f, -9.81f); @@ -678,8 +685,8 @@ void CcdPhysicsEnvironment::AddCcdGraphicController(CcdGraphicController *ctrl) ctrl, 0, // this object does not collision with anything 0, - nullptr, // dispatcher => this parameter is not used - 0)); + nullptr // dispatcher => this parameter is not used + )); BLI_assert(ctrl->GetBroadphaseHandle()); } @@ -1001,33 +1008,35 @@ void CcdPhysicsEnvironment::SetSolverType(PHY_SolverType solverType) return; } - switch (solverType) { - case PHY_SOLVER_SEQUENTIAL: - { - m_solver = new btSequentialImpulseConstraintSolver(); - break; - } - case PHY_SOLVER_NNCG: - { - m_solver = new btNNCGConstraintSolver(); - break; - } - case PHY_SOLVER_MLCP_DANTZIG: - { - m_solver = new btMLCPSolver(new btDantzigSolver()); - break; - } - case PHY_SOLVER_MLCP_LEMKE: - { - m_solver = new btMLCPSolver(new btLemkeSolver()); - break; - } - default: - { - BLI_assert(false); + for (unsigned short i = 0, size = m_solvers.size(); i < size; ++i) { + switch (solverType) { + case PHY_SOLVER_SEQUENTIAL: + { + m_solvers[i] = new btSequentialImpulseConstraintSolver(); + break; + } + case PHY_SOLVER_NNCG: + { + m_solvers[i] = new btNNCGConstraintSolver(); + break; + } + case PHY_SOLVER_MLCP_DANTZIG: + { + m_solvers[i] = new btMLCPSolver(new btDantzigSolver()); + break; + } + case PHY_SOLVER_MLCP_LEMKE: + { + m_solvers[i] = new btMLCPSolver(new btLemkeSolver()); + break; + } + default: + { + BLI_assert(false); + } } } - ; + m_solverType = solverType; } @@ -1964,48 +1973,8 @@ CcdPhysicsEnvironment::~CcdPhysicsEnvironment() { m_wrapperVehicles.clear(); - //m_broadphase->DestroyScene(); - //delete broadphase ? release reference on broadphase ? - - //first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher - //delete m_dispatcher; - delete m_dynamicsWorld; - - if (nullptr != m_ownDispatcher) { - delete m_ownDispatcher; - } - - if (nullptr != m_solver) { - delete m_solver; - } - - if (nullptr != m_debugDrawer) { - delete m_debugDrawer; - } - - if (nullptr != m_filterCallback) { - delete m_filterCallback; - } - - if (nullptr != m_ghostPairCallback) { - delete m_ghostPairCallback; - } - - if (nullptr != m_collisionConfiguration) { - delete m_collisionConfiguration; - } - - if (nullptr != m_broadphase) { - delete m_broadphase; - } - - if (nullptr != m_cullingTree) { - delete m_cullingTree; - } - - if (nullptr != m_cullingCache) { - delete m_cullingCache; - } + // First delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher. + m_dynamicsWorld.reset(nullptr); } btTypedConstraint *CcdPhysicsEnvironment::GetConstraintById(int constraintId) @@ -2530,7 +2499,7 @@ PHY_IConstraint *CcdPhysicsEnvironment::CreateConstraint(class PHY_IPhysicsContr PHY_IVehicle *CcdPhysicsEnvironment::CreateVehicle(PHY_IPhysicsController *ctrl) { const btRaycastVehicle::btVehicleTuning tuning = btRaycastVehicle::btVehicleTuning(); - BlenderVehicleRaycaster *raycaster = new BlenderVehicleRaycaster(m_dynamicsWorld); + BlenderVehicleRaycaster *raycaster = new BlenderVehicleRaycaster(m_dynamicsWorld.get()); btRaycastVehicle *vehicle = new btRaycastVehicle(tuning, ((CcdPhysicsController *)ctrl)->GetRigidBody(), raycaster); WrapperVehicle *wrapperVehicle = new WrapperVehicle(vehicle, raycaster, ctrl); m_wrapperVehicles.push_back(wrapperVehicle); diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h index d3621d723eff..f02da7344714 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h @@ -30,27 +30,31 @@ #include #include #include -class CcdGraphicController; #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" + +class PHY_IVehicle; +class CcdGraphicController; +class WrapperVehicle; +class CcdOverlapFilterCallBack; +class CcdShapeConstructionInfo; class btTypedConstraint; class btSimulationIslandManager; class btCollisionDispatcher; class btDispatcher; - -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - -class WrapperVehicle; +class btDefaultCollisionConfiguration; class btPersistentManifold; class btBroadphaseInterface; struct btDbvtBroadphase; +class btGhostPairCallback; class btOverlappingPairCache; class btIDebugDraw; class btDynamicsWorld; -class PHY_IVehicle; -class CcdOverlapFilterCallBack; -class CcdShapeConstructionInfo; +class btSoftRigidDynamicsWorldMt; +class btConstraintSolver; +class btConstraintSolverPoolMt; /// Find the id of the closest node to a point in a soft body. int Ccd_FindClosestNode(btSoftBody *sb, const btVector3& worldPoint); @@ -63,7 +67,6 @@ int Ccd_FindClosestNode(btSoftBody *sb, const btVector3& worldPoint); class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment, public mt::SimdClassAllocator { friend class CcdOverlapFilterCallBack; - btVector3 m_gravity; /// Removes the constraint and his references from the owner and the target. void RemoveConstraint(btTypedConstraint *con, bool free); @@ -75,15 +78,41 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment, public mt::SimdCla void RestoreConstraint(CcdPhysicsController *ctrl, btTypedConstraint *con); protected: + btVector3 m_gravity; + btIDebugDraw *m_debugDrawer; - class btDefaultCollisionConfiguration *m_collisionConfiguration; + std::unique_ptr m_collisionConfiguration; /// broadphase for dynamic world - class btBroadphaseInterface *m_broadphase; + std::unique_ptr m_broadphase; /// for culling only - btOverlappingPairCache *m_cullingCache; + std::unique_ptr m_cullingCache; /// broadphase for culling - struct btDbvtBroadphase *m_cullingTree; + std::unique_ptr m_cullingTree; + + /** use explicit btSoftRigidDynamicsWorld/btDiscreteDynamicsWorld* so that we have access to + * btDiscreteDynamicsWorld::addRigidBody(body,filter,group) + * so that we can set the body collision filter/group at the time of creation + * and not afterwards (breaks the collision system for radar/near sensor) + * Ideally we would like to have access to this function from the btDynamicsWorld interface + */ + std::unique_ptr m_dynamicsWorld; + + std::unique_ptr m_solverPool; + std::vector m_solvers; + std::unique_ptr m_solverMt; + + std::unique_ptr m_filterCallback; + + std::unique_ptr m_ghostPairCallback; + + std::unique_ptr m_dispatcher; + + std::set m_controllers; + std::vector m_wrapperVehicles; + + PHY_ResponseCallback m_triggerCallbacks[PHY_NUM_RESPONSE]; + void *m_triggerCallbacksUserPtrs[PHY_NUM_RESPONSE]; /// solver iterations int m_numIterations; @@ -100,6 +129,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment, public mt::SimdCla float m_contactBreakingThreshold; void ProcessFhSprings(double curTime, float timeStep); + virtual void ExportFile(const std::string& filename); public: CcdPhysicsEnvironment(PHY_SolverType solverType, bool useDbvtCulling); @@ -147,7 +177,6 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment, public mt::SimdCla void SimulationSubtickCallback(btScalar timeStep); virtual void DebugDrawWorld(); -// virtual bool proceedDeltaTimeOneStep(float timeStep); virtual void SetFixedTimeStep(bool useFixedTimeStep, float fixedTimeStep) { @@ -235,7 +264,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment, public mt::SimdCla btBroadphaseInterface *GetBroadphase(); btDbvtBroadphase *GetCullingTree() { - return m_cullingTree; + return m_cullingTree.get(); } btDispatcher *GetDispatcher(); @@ -244,12 +273,12 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment, public mt::SimdCla void SyncMotionStates(float timeStep); - class btSoftRigidDynamicsWorld *GetDynamicsWorld() + btSoftRigidDynamicsWorldMt *GetDynamicsWorld() { - return m_dynamicsWorld; + return m_dynamicsWorld.get(); } - class btConstraintSolver *GetConstraintSolver(); + btConstraintSolver *GetConstraintSolver(); void MergeEnvironment(PHY_IPhysicsEnvironment *other_env); @@ -267,33 +296,6 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment, public mt::SimdCla /* Set the rigid body joints constraints values for converted objects and replicated group instances. */ virtual void SetupObjectConstraints(KX_GameObject *obj_src, KX_GameObject *obj_dest, bRigidBodyJointConstraint *dat); - -protected: - std::set m_controllers; - - PHY_ResponseCallback m_triggerCallbacks[PHY_NUM_RESPONSE]; - void *m_triggerCallbacksUserPtrs[PHY_NUM_RESPONSE]; - - std::vector m_wrapperVehicles; - - /** use explicit btSoftRigidDynamicsWorld/btDiscreteDynamicsWorld* so that we have access to - * btDiscreteDynamicsWorld::addRigidBody(body,filter,group) - * so that we can set the body collision filter/group at the time of creation - * and not afterwards (breaks the collision system for radar/near sensor) - * Ideally we would like to have access to this function from the btDynamicsWorld interface - */ - // class btDynamicsWorld *m_dynamicsWorld; - class btSoftRigidDynamicsWorld *m_dynamicsWorld; - - class btConstraintSolver *m_solver; - - class CcdOverlapFilterCallBack *m_filterCallback; - - class btGhostPairCallback *m_ghostPairCallback; - - class btDispatcher *m_ownDispatcher; - - virtual void ExportFile(const std::string& filename); }; class CcdCollData : public PHY_ICollData diff --git a/source/gameengine/Physics/Common/PHY_ICharacter.h b/source/gameengine/Physics/Common/PHY_ICharacter.h index 79b4d0ebbe4e..9a87acd466f6 100644 --- a/source/gameengine/Physics/Common/PHY_ICharacter.h +++ b/source/gameengine/Physics/Common/PHY_ICharacter.h @@ -18,8 +18,8 @@ class PHY_ICharacter virtual void Jump() = 0; virtual bool OnGround() = 0; - virtual float GetGravity() = 0; - virtual void SetGravity(float gravity) = 0; + virtual mt::vec3 GetGravity() = 0; + virtual void SetGravity(const mt::vec3& gravity) = 0; virtual unsigned char GetMaxJumps() = 0; virtual void SetMaxJumps(unsigned char maxJumps) = 0;